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Abstract 


iii 


The  development  of  low-cost  sensors  has  generated  a  corresponding  movement  to  integrate  them  into 
many  different  applications.  One  such  application  is  determining  the  rotational  attitude  of  an  object. 
Since  many  of  these  low-cost  sensors  are  less  accurate  than  their  more  expensive  counterparts,  their  noisy 
measurements  must  be  filtered  to  obtain  optimum  results.  This  work  describes  the  development,  testing, 
and  evaluation  of  four  filtering  algorithms  for  the  nonlinear  sounding  rocket  attitude  determination 
problem.  Sun  sensor,  magnetometer,  and  rate  sensor  measurements  are  simulated.  A  quaternion 
formulation  is  used  to  avoid  singularity  problems  associated  with  Euler  angles  and  other  three-parameter 
approaches.  Prior  to  filtering,  Gauss-Newton  error  minimization  is  used  to  reduce  the  six  reference  vector 
components  to  four  quaternion  components  that  minimize  a  quadratic  error  function.  Two  of  the 
algorithms  are  based  on  the  traditional  extended  Kalman  filter  (EKF)  and  two  are  based  on  the  recently 
developed  unscented  Kalman  filter  (UKF).  One  of  each  incorporates  rate  measurements,  while  the  others 
rely  on  differencing  quaternions.  All  incorporate  a  simplified  process  model  for  state  propagation 
allowing  the  algorithms  to  be  applied  to  rockets  with  different  physical  characteristics,  or  even  to  other 
platforms.  Simulated  data  are  used  to  develop  and  test  the  algorithms,  and  each  successfully  estimates  the 
attitude  motion  of  the  rocket,  to  varying  degrees  of  accuracy.  The  UKF-based  filter  that  incorporates  rate 
sensor  measurements  demonstrates  a  clear  performance  advantage  over  both  EKFs  and  the  UKF  without 
rate  measurements.  This  is  due  to  its  superior  mean  and  covariance  propagation  characteristics  and  the 
fact  that  differencing  generates  noisier  rates  than  measuring.  For  one  sample  case,  the  “pointing  accuracy” 
of  the  rocket  spin  axis  is  improved  by  approximately  39  percent  over  the  EKF  that  uses  rate  measurements 
and  by  40  percent  over  the  UKF  without  rates.  The  performance  of  this  UKF-based  algorithm  is  evaluated 
under  other-than-nominal  conditions  and  proves  robust  with  respect  to  data  dropouts,  motion  other  than 
predicted  and  over  a  wide  range  of  sensor  accuracies.  This  UKF-based  algorithm  provides  a  viable  low 
cost  alternative  to  the  expensive  attitude  determination  systems  currently  employed  on  sounding  rockets. 
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1.0  Introduction 

1.1  Background 

The  use  of  sounding  rockets  for  scientific  and  engineering  investigation  reaches  back  several  decades. 
Over  those  many  years  it  has  progressed  from  the  challenge  of  simply  getting  a  rocket  off  the  ground  to 
the  use  of  the  advanced  research  platforms  represented  by  today’s  sounding  rockets.  Parallel  to  the 
development  of  more  reliable  and  higher  performance  rockets,  has  been  a  burgeoning  need  for  more 
capable  instrumentation  and  related  analysis  techniques.  One  area  that  has  evolved  substantially  over  time 
is  the  tracking  of  rocket  payloads.  This  particular  facet  of  analyzing  rocket  missions  began  with  “eye- 
balling”  the  rocket  to  discover  where  it  went.  With  the  advent  of  higher  performance  rockets,  this  was  no 
longer  an  adequate  technique  and  more  advanced  optical  methods,  such  as  those  based  on  telescopic 
cameras  and  theodolites,  were  developed.  Once  rockets  were  capable  of  traveling  beyond  visual  range, 
tracking  was  often  done  using  various  radar  methods.  While  radar  tracking  of  the  rocket  position  is  still 
the  principal  method  today,  the  use  of  Global  Positioning  System  (GPS)  techniques  is  making  significant 
inroads  [1],  [2],  As  a  result  of  continuing  development  in  this  area,  accurate  and  cost-effective  methods 
currently  exist  for  determining  the  position  of  a  rocket  and  its  payload. 

As  sounding  rocket  science  became  more  sophisticated,  it  was  no  longer  sufficient  to  track  only  the 
payload’s  position.  It  is  now  desirable,  and  in  many  cases  imperative,  to  also  estimate  the  payload’s 
orientation,  or  attitude  [3],  [4],  [5].  Most  early  efforts  focused  on  deterministic  methods  of  attitude 
determination,  referencing  the  rocket’s  orientation  to  known  objects  or  fields  that  could  be  sensed  from  the 
rocket  while  in  flight.  Since  any  measurement  has  some  uncertainty  or  error  associated  with  it,  techniques 
were  devised  to  minimize  these  measurement  errors  through  the  use  of  better  sensors  and  ever  more 
sophisticated  data  processing  and  filtering  algorithms.  In  fact,  parallel  development  of  attitude 
determination  systems  for  aircraft  and  missiles  produced  hardware  and  techniques  suitable  for  use  on 
sounding  rockets.  Unfortunately  for  the  experimenter  on  a  limited  budget,  many  of  these  are  based  on 
Inertial  Navigation  Systems  (INS)  that  require  very  accurate  components  and  have  a  corresponding  high 
cost.  Following  system  development  for  aircraft  and  missiles,  many  attitude  determination  techniques 
have  been  devised  for  spacecraft.  The  vast  majority  of  these  are  again  based  on  INS,  star  trackers,  and 
other  expensive  components.  Here  again,  these  efforts  have  focused  on  high  performance  at  relatively 
high  cost  due  to  the  overall  budget  and  operational  considerations  for  these  systems.  While  much  of  the 
hardware  can  be,  and  has  been,  adapted  to  sounding  rocket  use,  it  is  cost  prohibitive  for  many 
experimenters. 
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The  use  of  sounding  rockets  as  scientific  platforms  is  driven  by  a  number  of  considerations.  For  many 
experiments,  there  is  no  reasonable  alternative.  High-altitude  research  balloons  and  aircraft  cannot  reach 
sufficient  altitude,  while  low-Earth  orbit  satellites  are  still  too  high.  This  is  the  case  for  many  ionospheric 
and  upper-atmosphere  experiments,  for  instance.  For  missions  where  an  alternative  platform  does  exist, 
cost  is  often  the  driving  factor  for  choosing  sounding  rockets.  In  a  period  of  tight  budgets  for  both 
government  and  university  research  programs,  experimenters  are  looking  for  the  most  cost-effective 
method  to  accomplish  good  science.  Sounding  rockets  often  fulfill  this  requirement,  if  they  can  provide 
adequate  support  for  the  science  at  an  affordable  price.  As  discussed  above,  affordable  solutions  are 
already  available  in  terms  of  tracking  rocket  position.  What  is  often  lacking  is  affordable  support  in  terms 
of  accurately  determining  the  payload’s  spatial  orientation,  or  attitude. 

This  dissertation  examines  past  approaches  to  this  problem,  available  sensors  for  this  application,  relevant 
sensor  fusion  techniques,  and  promising  filtering  approaches.  The  core  objective  is  to  leverage  sensor 
fusion  and  filtering  techniques  to  design  and  implement  an  algorithm  that  allows  for  accurate,  cost- 
effective  attitude  determination  using  sounding  rocket  class  sensors. 

1.2  Potential  Applications 

The  primary  focus  of  this  effort  is  the  sounding  rocket  attitude  determination  problem.  There  is  ample 
evidence  of  the  shortcomings  in  this  particular  area.  The  NASA  Goddard  Space  Flight  Center  (GSFC) 
Wallops  Flight  Facility  (WFF)  is  responsible  for  the  NASA  sounding  rocket  program.  While  they  no 
longer  carry  out  the  operational  program,  they  monitor  the  on-site  contractor,  do  special  analyses,  support 
range  safety,  etc.  The  Guidance,  Navigation,  and  Control  Systems  Engineering  Branch  at  WFF  has 
identified  ‘‘assembling  various  ‘low  quality’  attitude  sources  from  sounding  rocket  class  attitude  sensors  to 
produce  a  better  solution  to  provide  to  the  scientist”  as  a  research  interest  [6],  The  current  solution  for 
experimenters  flying  missions  with  WFF  is  one  of  several  contractor-provided  INS-based  systems  that 
must  be  integrated  into  the  rocket  payload.  These  expensive  systems  are  essentially  “rented”  and  then 
returned  to  the  contractor  for  post-flight  refurbishment  [7].  Another  current  indicator  of  a  valid 
requirement  for  improved  low-cost  attitude  determination  is  the  Horizontal  E-region  Experiment  (HEX) 
mission  launched  in  the  spring  of  2003.  Pre-launch  program  reviews  of  this  joint  NASA/United  States 
Geophysical  Institute/University  of  Alaska  Fairbanks  project  highlighted  the  difficulty  of  the  attitude 
determination  problem  [8].  It  is  possible  that  the  data  processing  algorithm  proposed  in  this  thesis,  in 
combination  with  the  sensors  available  for  that  mission,  might  have  allowed  for  adequate  accuracy  without 
the  expensive  gyro-based  system  eventually  used.  At  the  very  least,  this  approach  could  have  provided 
experimenters  a  quick-look  assessment  of  rocket  payload  attitude  while  the  more  accurate  data  from  the 
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contractor-provided  system  was  being  analyzed.  In  addition  to  these  specific  program  needs,  there  is  a 
more  general  impetus  for  this  work.  A  number  of  universities  currently  support  sounding  rocket  programs 
to  conduct  both  instruction  in  engineering  design  and  scientific  investigation  [9],  [10].  The  university 
programs  are  not  typically  funded  at  the  level  necessary  to  incorporate  expensive  attitude  determination 
systems  into  their  missions.  As  a  result,  the  type  and  quality  of  possible  research  is  limited.  Development 
of  a  cost-effective  system  would  allow  not  only  attitude  determination,  but  will  also  serve  as  a  stepping 
stone  to  adding  attitude  or  pointing  control  capabilities.  Even  for  non-university  programs  that  enjoy  more 
robust  budgets,  an  accurate  low-cost  system  is  desirable  because  it  frees  up  budget  dollars  that  can  be 
spent  elsewhere. 

While  sounding  rockets  are  clearly  the  focus  here,  the  algorithm  designed  for  this  effort  is  widely 
applicable.  Certain  provisions  are  made  to  enable  the  system  to  handle  the  high-dynamic  case  of  rocket 
flight,  but  there  is  no  reason  that  the  algorithm  cannot  be  applied  to  lower  dynamic  applications  such  as 
submersibles,  land-based  vehicles,  aircraft,  etc.  Any  and  all  of  these  can  benefit  from  low-cost,  robust  and 
accurate  attitude  determination. 

1.3  Research  Questions  and  Contributions  of  This  Work 

This  thesis  will  examine  the  following  research  topics: 

-  Evaluate  prominent  methods  for  representing  attitude  in  conjunction  with  sounding  rocket  attitude  filter 
design; 

-  Evaluate  likely  sensor  fusion  and  filtering  techniques  for  use  in  sounding  rocket  attitude  filter  design; 

-  Leverage  the  most  suitable  filtering  and  attitude  representation  methods  to  design  a  filter  that  estimates 
rocket  attitude  based  on  information  provided  by  a  minimal  low-cost  attitude  sensor  set; 

-  Demonstrate  filter  performance  using  simulated  data  with  an  emphasis  on  capability  relative  to  reduced 
sensor  cost. 

The  chapters  that  follow  review  other  work  related  to  this  topic  and  detail  the  approach  to  investigating  the 
research  questions  noted  above.  It  is  determined  that  a  quaternion  formulation  of  the  attitude  representing 
the  relationship  between  a  body  fixed  frame  and  the  Earth-Centered-Inertial  frame  is  the  most  useful 
representation.  A  minimal  sensor  suite  composed  of  Sun  sensors,  magnetometers,  and  rate  sensors  is 
defined  and  Gauss-Newton  error  minimization  provides  the  initial  data  fusion  for  combining  the  measured 
reference  vectors  into  an  attitude  quaternion.  As  a  result,  the  measurements  input  to  the  filter  portion  of 
the  algorithm  are  reduced  in  number  from  nine  to  seven.  The  seven  measurements  are  three  body  rates 
and  four  quaternion  components.  Four  filtering  algorithms  are  evaluated  by  using  each  to  process 
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identical  simulated  data  sets.  The  four  algorithms  are  an  Extended  Kalman  Filter  (EKF)  operating  on  rate 
information  derived  by  differencing  quaternions,  an  EKF  that  incorporates  rate  measurements  from  rate 
sensors,  an  unscented  Kalman  filter  (UKF)  operating  on  rate  information  derived  by  differencing 
quaternions,  and  a  UKF  incorporating  rate  measurements  from  rate  sensors.  The  UKF  incorporating  rate 
measurements  is  shown  to  outperform  the  others  and  is  demonstrated  to  be  capable  of  successfully 
estimating  the  attitude  state  vector  under  a  wide  range  of  conditions. 

This  work  represents  the  first  known  application  of  the  recently  developed  UKF  to  the  rocket  attitude 
determination  problem.  It  demonstrates  side-by-side  comparison  of  EKF  and  UKF  performance  under 
identical  experimental  conditions  and  develops  a  user-friendly  test  bed  for  further  evaluation  of  predicted 
filter  performance  under  numerous  conditions.  It  demonstrates  the  successful,  albeit  at  degraded  accuracy, 
estimation  of  rocket  attitude  motion  using  derived  rate  information  in  place  of  rates  measured  by  rate 
gyroscopes  or  other  means.  A  method  is  demonstrated  for  detecting  anomalous  filter  operation  in  real 
world  applications.  Finally,  it  provides  a  viable  alternative  to  the  traditional,  expensive,  attitude 
determination  methods  by  incorporating  an  approach  suitable  for  implementation  using  low  cost  sensors. 
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2.0  Previous  Work 

The  problem  of  state  estimation  has  been  addressed  by  many  researchers  using  a  wide  variety  of  methods. 
What  makes  this  effort  unique  is  its  focus  on  a  solution  for  “low  cost”  systems  using  less  than  optimum 
sensors.  The  application  of  interest,  sounding  rockets,  also  tailors  the  research  and  guides  the  selection  of 
sensors,  data  fusion  method,  and  filtering  approach.  The  following  two  sections  provide  a  brief  survey  of 
the  work  to  date  that  is  most  closely  related  to  the  goals  for  this  research. 

2.1  Solutions  for  Low-Cost  Applications  in  General 

The  work  most  closely  aligned  with  this  effort  is  reported  in  [11]  and  [12].  Here  the  authors  investigate 
the  filtering  of  attitude  data  based  on  a  low-dynamics  application.  Their  work  describes  a  quaternion- 
based  filter  that  relies  on  error  minimization  to  convert  sensor  measurements  into  a  quaternion 
parameterization  of  attitude.  The  sensor  suite  evaluated  is  composed  of  magnetometers,  angular  rate 
sensors,  and  accelerometers  combined  into  a  single  package  sensor  known  as  a  MARG  sensor,  where 
MARG  stands  for  Magnetic,  Angular  Rate,  and  Gravity.  The  data  filtering  algorithm  is  based  upon  an 
extended  Kalman  filter  approach.  While  the  filter  incorporates  some  simplifying  assumptions,  it  is 
essentially  a  traditional  extended  Kalman  filter  design.  Their  goal,  like  one  of  the  goals  here,  is  real-time 
estimation  of  orientation.  The  application  of  interest,  however,  is  body  motion  simulation  for  insertion 
into  virtual  reality  scenarios.  The  algorithm  is  demonstrated  to  successfully  track  orientation  changes  at 
rates  on  the  order  of  0.05  rad/sec,  meant  to  simulate  human  body  motion,  but  much  lower  than  those 
encountered  during  a  rocket  flight. 

In  [13],  quaternions  are  again  used  as  the  desired  parameterization  of  attitude,  and  an  algorithm  is 
designed  to  estimate  orientation  from  gravity  and  magnetic  field  measurements.  Error  minimization 
techniques  are  used  to  reduce  the  number  of  observations  presented  to  the  filter  and  the  algorithm  is  based 
on  a  complementary  filter  instead  of  a  Kalman  filter,  in  an  attempt  to  avoid  the  tuning  required  for  Kalman 
type  filters.  The  work  described  concentrates  on  avoiding  singularities  and  reducing  the  computational 
burden  through  a  “reduced  order”  approach.  While  the  successful  convergence  of  the  error  minimization 
is  reported,  the  estimation  algorithm  is  discussed  from  a  theoretical  perspective  without  results. 

The  authors  in  [14]  report  on  the  design  of  another  quaternion-based  attitude  determination  system  that 
focuses  on  low  cost  sensors.  In  this  work,  magnetic  field  and  gravity  are  again  the  two  measured 
quantities,  supplemented  by  acceleration  derived  from  Global  Positioning  System  (GPS)  measurements. 
An  iterated  least  squares  approach  is  used  in  an  error  minimization  routine  to  estimate  an  error  quaternion 
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and  an  extended  Kalman  filter  is  the  basis  for  the  filtering  approach.  In  this  algorithm,  the  error 
quaternion  is  estimated  instead  of  estimating  the  attitude  directly.  The  application  of  interest  is  aircraft 
attitude  determination,  and  the  focus  is  on  eliminating  the  need  for  rate  measurements,  specifically 
gyroscopes.  By  processing  both  simulated  and  actual  aircraft  data,  they  demonstrate  successful 
convergence  of  the  error  minimization  routine  and  that  the  algorithm  is  able  to  successfully  track  aircraft 
orientation. 

Bachmann  et  al.  details  yet  another  orientation  filter  based  on  quaternions,  only  in  this  case  the  filtering 
algorithm  is  based  on  the  “complementary”  filter  approach  and  uses  magnetic  field,  gravity  measurements, 
and  angular  rate  measurements  as  its  inputs  [15].  These  measurements  come  from  MARG  sensors  which 
were  described  earlier,  and  the  application  of  interest  is  estimation  of  body  posture  based  on  limb 
orientation.  A  Gauss-Newton  error  minimization  technique  is  used  to  reduce  the  number  of  sensor 
“measurements”  fed  into  the  filter.  The  authors  report  initial  testing  of  the  algorithm  demonstrated 
successful  orientation  tracking  at  rates  from  10  to  30  deg/sec. 

While  each  of  these  efforts  incorporates  tools  that  may  be  useful  for  estimating  the  attitude  of  a  sounding 
rocket,  each  relies  on  conditions  that  are  violated  by  the  high-dynamic  nature  of  a  rocket’s  operating 
environment.  Of  the  works  described,  the  application  with  an  environment  most  closely  matched  to  that 
envisioned  in  this  work  is  the  estimation  of  aircraft  attitude.  In  this  case,  GPS  measurements  were 
differenced  to  get  a  separate  acceleration  measurement  with  which  to  determine  what  component  of  the 
accelerometer  measurements  were  due  to  aircraft  motion  and  what  part  were  due  to  gravity.  Such  a  GPS 
augmentation  would  be  difficult  to  implement  on  a  rocket  that  experiences  very  high  linear  acceleration 
and  rotational  rates.  Given  the  limitations  of  these  approaches  relative  to  the  sounding  rocket  application, 
the  next  section  describes  work  that  has  been  done  with  respect  to  rockets  in  particular. 

2.2  Work  on  Rockets 

It  is  difficult  to  find  work  that  has  been  tailored  specifically  to  the  rocket  attitude  determination  problem. 
The  works  of  Zarchan  [16],  [17],  Musoff  [17],  and  others  address  the  position  determination/estimation 
problem  to  significant  depth,  but  most  work  on  attitude  determination  of  rockets  has  focused  on 
deterministic  methods,  or  how  to  best  determine  the  attitude  at  a  given  time  step.  The  compilation  by 
Wertz  [18]  and  Smith’s  [19]  discussion  of  methods  for  spinning  spacecraft  are  exceptional  references  for  a 
general  overview  of  the  attitude  determination  problem.  References  [3]  and  [4]  present  deterministic 
methods  for  determining  sounding  rocket  attitude  given  measurements  from  a  Sun  sensor  and  a 
magnetometer  at  a  given  time  step,  but  no  filtering  is  employed  to  improve  the  estimate.  Lai  addresses  a 
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unique  case  in  which  magnetic  field  was  measured  in  only  two  axes  and  additional  measurements  were 
provided  by  a  Moon  sensor  [20].  Here  again,  no  filtering  was  employed.  Koehler  et  al.  describe  a 
deterministic  approach  that  uses  telemetry  signal  strength  in  conjunction  with  magnetic  field  aspect  data 
[5]. 

Challa  and  Natanson  [21],  Crassidis  and  Markley  [22],  Murrell  [23],  and  many  others  have  designed 
Kalman-based  filters  to  estimate  attitude  of  spacecraft,  but  most  of  these  efforts  involve  sensor  suites  not 
well-suited  to  rocket  missions,  and  all  of  them  estimate  motion  in  a  very  different  operating  environment 
than  that  expected  for  a  sounding  rocket.  Challa  and  Natanson  describe  a  system  using  only  Kalman 
filtered  magnetometer  data  for  spacecraft  attitude  determination,  but  note  that  the  magnetic  field  must 
change  rapidly  enough  to  calculate  a  time  derivative  [21].  This  would  not  be  the  case  for  most  sounding 
rocket  flights.  The  algorithm  also  has  very  long  convergence  times  which  would  not  be  acceptable. 
Crassidis  and  Markley  describe  a  method  capable  of  attitude  estimation  without  rate  measurements  based 
on  Kalman  filtering  and  error  minimization,  but  it  is  tailored  to  the  three-axis  stabilized  satellite  case  [22], 
The  method  in  [23]  processes  data  from  an  inertial  reference  unit  and  star  trackers,  exemplifying  the  many 
applications  using  sensors  beyond  the  reach  of  a  “low  cost”  program.  There  are  numerous  such  efforts, 
along  the  lines  of  those  mentioned  in  this  brief  review,  that  are  described  in  the  literature  and  [24]  gives  a 
very  thorough  synopsis  of  Kalman  filtering  as  applied  to  spacecraft  attitude  estimation.  As  was  the  case 
for  the  methods  described  in  the  last  section,  many  of  these  works  embody  tools  that  may  be  applied  to  the 
estimation  of  sounding  rocket  attitude,  but  none  lends  itself  directly  to  this  somewhat  unique  application. 

Based  on  this  review  of  work  to  date,  it  is  evident  that  there  is  a  niche  to  be  explored  relative  to  estimating 
rocket  attitude  using  low-cost  sensors.  Having  reviewed  the  most  pertinent  work  to  date,  the  next  chapter 
begins  to  lay  out  the  tools  necessary  to  describe,  measure,  and  estimate  the  attitude  of  a  rigid  body 
undergoing  rotational  motion.  Chapter  3  discusses  suitable  coordinate  frames.  Chapter  4  describes  what 
parameters  best  define  the  attitude.  Chapter  5  reviews  rotational  motion  and  how  it  is  best  modeled  for  a 
low  cost  estimation  routine,  and  Chapter  6  builds  a  case  for  what  sensors  are  best  suited  for  measuring 
rocket  attitude.  The  remaining  chapters  discuss  data  fusion  approaches,  filtering  techniques,  algorithm 
design,  and  algorithm  performance. 
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3.0  Suitable  Coordinate  Axes 

3.1  Introduction 

In  all  dynamics  work  it  is  crucial  to  use  well-defined  coordinate  frames.  The  motion  of  interest  is 
referenced  to  these  frames  and  a  well-chosen  frame  will  often  simplify  the  calculations  involved  as  well  as 
the  physical  understanding  of  the  problem.  There  are  a  number  of  frames  that  are  considered  most  suitable 
for  certain  classes  of  problems.  In  their  discussion  of  the  spacecraft  attitude  determination  problem, 
Fortescue  and  Stark  point  out  that  “attitude”  and  “orientation”  are  usually  easily  understood  concepts  in 
terms  of  frames. 

There  must  be  some  datum  frame  of  reference,  and  once  this  has  been  chosen  then  the 
attitude  of  a  spacecraft  refers  to  its  angular  departure  from  this  datum.  A  right-handed  set 
of  axes  is  normally  used  in  order  to  define  a  frame  of  reference,  and  if  both  a  datum  set 
and  a  set  of  spacecraft  axes  are  chosen,  then  the  attitude  may  be  defined  in  a  way  that 
may  be  quantified  [25]. 

The  next  sections  will  review  the  common  coordinate  axes  and  examine  which  frames  are  best-suited  to 
the  sounding  rocket  attitude  determination  problem. 

3.2  Common  Coordinate  Frames 

In  attitude  dynamics  work,  there  are  a  number  of  “typical”  frames  encountered.  First,  and  fundamentally, 
there  is  defined  an  inertial  reference  frame.  This  is  important  because  it  is  only  in  an  inertial,  or  non¬ 
accelerating  reference  frame,  that  Newton’s  laws  of  motion  are  approximately  correct  [26].  Most  often 
this  “inertial”  frame  is  considered  to  be  one  referenced  to  the  “unmoving”  stars.  As  is  commonly  known 
today,  the  stars  are  indeed  moving  and  there  most  likely  is  no  truly  inertial  frame.  Most  problems  are 
therefore  worked  relative  to  a  “sufficiently  inertial”  reference  frame.  As  Kaplan  points  out,  “practical 
situations  dictate  only  that  the  inertial  frame  be  a  reference  coordinate  set  which  guarantees  required 
accuracy  over  the  time  interval  of  interest  [27].”  In  other  words,  even  though  all  real  frames  are  indeed 
moving  and  therefore  not  inertial,  for  practical  problems,  it  is  sufficient  to  select  a  coordinate  frame  which 
is  not  accelerating  enough  to  disturb  the  problem  solution  beyond  desired  accuracy  over  the  time  frame  of 
interest.  For  a  low-dynamics  problem,  this  sufficiently  inertial  frame  might  be  a  “local-level”  frame,  tied 
to  some  nearby  geographic  feature  and  having  coordinate  directions  of,  perhaps,  north-east-down  to  create 
a  right-handed  frame.  This  might  be  suitable  for  something  like  a  ground  vehicle  or  a  submersible.  For  a 
vehicle  with  higher  dynamics,  such  as  an  aircraft,  this  may  still  be  suitable  if  the  motion  is  constrained  to  a 
local  area  and  a  relatively  short  duration.  If  the  motion  will  continue  for  an  extended  period,  however,  and 
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over  great  distance  or  at  high  altitude,  an  Earth-centered  coordinate  system  might  be  more  appropriate. 
Finally,  if  we  are  analyzing  the  motion  of  a  spacecraft  or  rocket,  then  a  coordinate  frame  referenced  to  the 
Sun  or  stars  might  be  most  applicable.  Again,  what  is  “sufficiently”  inertial  will  depend  on  the  problem  at 
hand,  and  as  Kaplan  described,  the  true  test  is  whether  the  chosen  frame  accelerates  to  such  a  degree  that 
the  desired  accuracy  of  the  solution  is  impacted. 

Clearly,  since  it  is  the  orientation  of  a  particular  object  that  is  to  be  analyzed,  another  frame  of  interest 
may  be  envisioned  tied  to  the  body,  often  called  the  “body  frame.”  This  coordinate  frame  typically  has  its 
origin  at  the  vehicle  center  of  mass  and  is  a  convenient  coordinate  system  for  developing  the  equations  of 
motion  of  a  vehicle  [26].  A  familiar  aircraft  example,  described  by  Siouris,  is  to  choose  the  body  x-axis 
pointing  along  the  aircraft’s  longitudinal  axis  (the  roll  axis),  the  body  y-axis  out  the  right  wing  (the  pitch 
axis),  and  the  body  z-axis  pointing  down  (the  yaw  axis)  [26].  The  alignment  of  the  frame  axes  with  axes 
of  symmetry  of  the  object  or  with  its  principle  axes  serves  to  simplify  the  description  of  orientation  or 
motion.  This  frame  allows  for  the  orientation  of  the  object,  body,  or  vehicle  to  be  described  relative  to  the 
sufficiently  inertial  frame  in  which  Newton’s  laws  of  motion  may  be  applied. 

A  third  type  of  coordinate  frame  that  is  frequently  encountered  in  attitude  work,  is  one  centered  at  and  tied 
to  a  particular  sensor.  In  some  situations,  it  is  convenient  to  define  such  a  coordinate  frame  to  simplify 
interpretation  and  manipulation  of  sensor  data.  There  may  be  such  a  frame  defined  for  each  sensor  and 
these  frames  may  then  be  related  to  the  overall  “body”  coordinate  frame  through  a  coordinate 
transformation.  This  body  frame  is  then  related,  through  another  coordinate  transformation,  to  the 
“inertial”  frame.  It  is  this  last  coordinate  transformation  that  defines  the  body’s  “attitude”  or  spatial 
orientation.  It  is  the  determination  of  this  transformation  relating  the  body  frame  to  the  inertial  frame  that 
is  the  goal  of  this  research  effort. 

3.3  Frames  Best  Suited  to  this  Problem:  Navigation  Frame  and  Body  Frame 

Since  the  “sensor  frames”  for  a  particular  application  may  or  may  not  differ  from  the  body  frame  and  will 
be  vehicle  specific,  this  work  concentrates  on  the  two  coordinate  frames  that  are  necessary  in  every  case. 
A  sufficiently  inertial  reference  frame  and  a  body  fixed  frame.  As  mentioned  earlier,  there  are  a  number 
of  candidates  for  an  “inertial”  frame.  The  most  obvious  candidates  are  a  local-level  frame,  an  Earth- 
Centered-Earth-Fixed  (ECEF)  frame,  an  Earth-Centered-Inertial  (ECI)  frame,  and  possibly  a  Sun- 
Centered-Inertial  frame.  Sounding  rockets  are  highly  dynamic  vehicles  that  may  remain  aloft  for 
substantial  periods  and  cover  large  distances.  This  profile  helps  eliminate  the  first  two  candidates  as 
“sufficiently”  inertial.  While  none  of  the  candidates  is  truly  inertial,  it  is  clear  that  the  local  level  frame 
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and  the  ECEF  frame  will  move  significantly  during  a  typical  flight.  If  a  typical  flight  is  assumed  to  last 
even  30  minutes  including  powered  flight,  coast  phase  and  time  under  parachute,  the  Earth  will  rotate 
approximately  seven  and  a  half  degrees  on  its  spin  axis.  In  the  case  of  a  local  level  frame,  perhaps  with  its 
origin  fixed  at  the  launch  site  on  or  near  the  surface  of  the  Earth,  the  origin  is  describing  an  arc  at  a 
tangential  velocity  approximately  equal  to 

V  =  /?  0)  cos  X  (3.1) 

tang  ® 

where 

Vtang  =  tangential  velocity  of  launch  site  traveling  west  to  east  (m/sec), 

R  =  radius  of  the  Earth  (m), 

© 

a  -  rotational  rate  of  the  Earth  (rad/sec), 

X  -  geographic  latitude  of  the  launch  site  (deg). 

To  a  rough  approximation,  this  calculation  yields  an  eastward  velocity  of  the  Earth’s  surface  at  the  equator 
of  about  460  meters  per  second.  Even  at  high  latitudes,  such  as  at  Poker  Flat  research  range  in  Alaska  at 
latitude  of  approximately  65  degrees  North,  this  eastward  velocity  is  in  excess  of  200  meters  per  second. 
Clearly  this  moving,  and  accelerating,  origin  is  not  “sufficiently  inertial.”  The  second  candidate,  the 
ECEF  frame,  suffers  by  a  similar  analysis.  While  the  velocity  of  the  origin,  tied  to  the  center  of  the  Earth 
in  its  path  around  the  Sun,  is  not  likely  to  impact  solution  accuracy,  the  fact  that  the  frame  is  fixed  to  the 
Earth  and  rotates  with  it  means  that  it  too  will  rotate  through  approximately  seven  and  a  half  degrees 
during  a  typical  30  minute  flight.  This,  too,  is  a  significant  change  in  the  frame  and  as  a  result  the  ECEF 
frame  is  not  suitable  for  a  rocket  dynamics  problem.  The  final  two  candidates  are  far  more  “fixed”  than 
the  first  two.  The  ECI  frame  has  its  origin  fixed  to  the  center  of  the  Earth  and  its  first  axis  pointed  to  the 
“first  point  of  Aries,”  a  “fixed”  star,  at  the  time  of  the  vernal  equinox  for  a  particular  time  epoch.  This 
axis  lies  in  the  Earth’s  equatorial  plane.  The  second  axis  is  also  in  the  equatorial  plane  and  is  90  degrees 
eastward  from  the  first.  The  third  axis  points  to  the  Earth’s  north  pole  and  completes  the  right-handed 
system.  During  a  typical  30  minute  flight,  the  movement  of  the  star  reference  is  virtually  imperceptible, 
and  to  a  rough  approximation,  the  Earth-centered  origin  will  move  on  the  order  of  .0004  radians  around 
the  Sun.  As  pointed  out  by  Herbert,  Wertz  and  others,  this  coordinate  system  is  not  truly  inertial  due  to 
the  slight  translation  and  rotation  of  the  frame,  but  is  typically  deemed  to  be  sufficiently  inertial  for 
spacecraft  and  rocket  orbital  and  attitude  work  [4],  [18],  [26].  The  remaining  candidate  is  similar  to  the 
ECI  frame,  except  that  its  origin  is  at  the  center  of  the  Sun  and  the  third  axis  is  normal  to  the  plane  of  the 
ecliptic  instead  of  the  Earth’s  equatorial  plane.  While  this  frame  is  even  more  inertial  than  the  ECI  frame, 
for  spacecraft  and  rocket  work  near  the  Earth,  the  increased  accuracy  of  the  solution  does  not  outweigh  the 
increased  complexity  of  the  calculations  involved.  Also,  since  many  of  the  reference  fields  that  are 
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detected  by  attitude  sensors  are  more  easily  defined  in  the  ECI  frame,  this  is  a  more  convenient  choice. 
The  ECI  frame  that  will  be  used  as  the  “sufficiently  inertial”  frame  is  depicted  in  Figure  3.1  below. 


vernal  equinox 
direction 

Figure  3.1:  Earth-Centered-Inertial  Coordinate  Frame  (After  Figure  2.2-2  in  [28]) 

Since  attitude  will  be  defined  as  a  transformation  relating  the  orientation  of  the  body  relative  to  the  inertial 
frame,  it  is  necessary  to  define  a  body-fixed  frame  as  described  earlier.  While  this  frame  may  be  defined 
anywhere  within  the  object  of  interest,  a  well  thought  out  choice  simplifies  the  calculations  and  physical 
understanding  of  the  motion.  The  origin  is  typically  defined  to  be  at  the  object’s  center  of  mass.  For  a 
rocket  problem,  there  is  typically  a  spin  axis  coinciding  with  the  longitudinal  axis  of  the  vehicle.  This  is  a 
natural  choice  for  one  of  the  body-fixed  axes.  The  other  two  axes  are  normal  to  this  one  in  order  to  define 
an  orthogonal,  right-handed  system.  While  choice  of  the  second  direction  is  arbitrary,  as  long  as  it 
satisfies  the  orthogonality  constraint,  it  is  often  chosen  to  align  with  some  sensor  view  direction  or  another 
physical  attribute  of  the  vehicle  (i.e.,  fin  #  1,  etc.).  As  this  body  frame  will  be  unique  to  a  particular 
application,  it  will  be  treated  in  a  general  sense  for  this  research  effort.  Historically,  several  choices  have 
been  common.  Originally,  some  researchers  chose  the  “x”  axis  to  correspond  with  the  longitudinal  axis 
[29].  A  more  contemporary  example,  the  space  shuttle,  also  designates  the  “x”  axis  to  be  out  the  nose,  the 
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“y”  axis  out  the  right  wing  and  the  “z”  axis  out  pointed  opposite  the  cargo  bay,  or  in  the  “down”  direction. 
In  works  dealing  primarily  with  spinning  objects,  as  many  rockets  and  satellites  are  designed  to  do,  the 
spin  axis  has  often  been  designated  as  the  “z”  axis  [30],  [31],  [32].  Again,  this  choice  is  somewhat 
arbitrary,  depending  on  the  problem.  For  this  research  effort,  the  body  frame  will  be  defined  as  a  right- 
handed  orthogonal  reference  frame  with  its  origin  at  the  vehicle  center  of  mass  and  with  the  third,  or  “z” 
axis,  aligned  with  the  principle  longitudinal  axis  of  the  rocket,  positive  direction  out  the  nose.  A 
representative  body-fixed  coordinate  frame  is  illustrated  in  Figure  3.2  below. 

y 


Figure  3.2:  Representative  Body-Fixed  Coordinate  Frame  (After  Figure  7.9-1  in  [30]) 

Sensor  axes  will  not  be  defined  for  this  effort.  It  is  assumed  that  the  transformations  to  relate  the  sensor 
axes  to  the  body  coordinate  frame  are  perfectly  known  and  that  no  additional  error  is  incurred  as  a  result  of 
inaccuracies  in  these  transformations.  Obviously,  a  physical  vehicle  will  have  errors  associated  with  these 
transformations  due  to  manufacturing  and  mounting  tolerances,  etc.  These  errors,  although  hopefully 
small,  would  have  to  be  included  in  an  overall  error  budget. 

Devising  an  algorithm  to  determine  rocket  attitude  involves  not  only  a  definition  of  coordinate  frames,  but 
also  a  selection  of  how  to  parameterize  the  attitude  and  thereby  the  transformation  between  the  frames. 
There  are  numerous  approaches  to  this,  with  a  few  being  relatively  common.  Each  has  its  advantages  and 
disadvantages,  and  the  next  sections  will  examine  the  more  common  methods  and  select  the  one  best- 
suited  to  the  sounding  rocket  attitude  determination  problem. 
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4.0  Coordinate  Parameterizations 

4.1  Introduction 

As  laid  out  in  the  last  chapter,  the  minimum  set  of  frames  required  to  analyze  attitude  or  orientation 
includes  a  “sufficiently  inertial”  frame  in  which  Newton’s  laws  approximately  hold  true,  and  a  body  frame 
fixed  to  the  object.  Since  attitude  motions  are  normally  represented  in  the  spacecraft  body  fixed  frame, 
which  is  rotating  and  accelerating,  the  subject  of  relative  motion  and  transformation  of  coordinates  plays 
an  important  role  in  attitude  dynamics  [33].  Historically,  spatial  attitude  descriptions  have  been 
accomplished  using  a  number  of  different  coordinate  parameterizations  [26],  [34],  [35],  [36],  [37],  [38]. 
The  following  paragraphs  discuss  the  parameters  most  commonly  appearing  in  the  literature,  their 
advantages  and  disadvantages  relative  to  this  application,  and  the  choice  of  parameterization  for  this 
research. 

4.2  Common  Coordinate  Parameterizations 

The  definition  of  an  object’s  attitude  may  be  distilled  down  to  the  description  of  the  transformation 
between  the  body-fixed  frame  and  the  datum  frame  [34].  This  transformation  may  be  described  by  one  of 
many  coordinate  parameterizations.  At  the  root  of  these  parameterizations,  attitude  can  be  viewed  from 
either  a  rectangular  or  a  spherical  coordinate  perspective.  While  spherical  coordinates  often  help  when 
visualizing  the  physical  motion  of  an  object,  their  manipulation  involves  the  heavy  use  of  trigonometric 
functions,  which  are  computationally  more  intensive  from  a  computing  point  of  view  [18].  In  general,  the 
availability  of  faster  computer  chips  has  made  this  less  of  an  issue.  However,  since  the  focus  of  this  work 
is  on  low  cost  systems,  available  processing  power  may  be  limited.  In  light  of  the  desire  to  implement  the 
algorithm  in  real  time,  the  computational  savings  associated  with  using  rectangular  coordinates  make  their 
use  worthwhile.  Even  after  narrowing  the  approach  to  one  based  on  rectangular  coordinates,  there  are  a 
large  number  of  coordinate  parameterizations  that  have  been  studied  and  used  for  attitude  determination 
work  [35].  From  this  large  group,  a  few  stand  out  as  the  most  suitable  for  the  sounding  rocket  problem. 

The  parameterization  considered  by  some  to  be  the  “fundamental”  representation  is  the  familiar  direction 
cosine  matrix  [37].  This  matrix  maps  vectors  in  the  datum  frame  to  the  body  frame,  or  vice  versa,  by 
employing  a  set  of  three  direction  cosine  elements  to  determine  the  orientation  of  each  inertial  axis  with 
respect  to  the  three  axes  of  the  body  frame  [26].”  Even  though  direction  cosines  can  be  considered  the 
“standard”  by  which  other  representations  might  be  judged,  Markley  points  out  that  other 
parameterizations  might  have  significant  advantages  in  certain  applications  [37].  One  disadvantage  of  the 
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direction  cosine  matrix  is  that  its  3x3  matrix  structure  requires  nine  elements.  Since  an  orientation  or 
rotation  of  a  rigid  body  possesses  only  three  degrees  of  freedom,  this  method  requires  six  constraints  to  get 
from  nine  parameters  to  the  minimum  of  three  that  are  theoretically  required.  While  this  method  is 
relatively  easy  to  understand  and  well  adapted  to  computer  programming  [3],  in  the  end  the  direction 
cosine  method  requires  the  manipulation  of  expensive  trigonometric  functions  and  imposes  a  computer 
storage  burden  that  is  not  required  when  using  some  alternative  methods. 

While  direction  cosines  may  be  the  best  known  method  for  describing  orientation,  a  second  often-used 
method  relies  on  Euler  angles  [12].  Since  any  rotation  of  a  rigid  body  may  be  completely  described  using 
only  three  parameters  [37],  it  is  very  appealing  to  choose  a  method  that  uses  this  minimum  set.  The  Euler 
angle  approach  is  perhaps  the  best  known  such  method.  Devised  by  Euler,  this  theorem  states  that  “Any 
two  independent  orthonormal  coordinate  frames  can  be  related  by  a  sequence  of  rotations  ( not  more  than 
three)  about  coordinate  axes,  where  no  two  successive  rotations  may  be  about  the  same  axis  [38].”  These 
three  rotations  made  in  a  precise  order  about  specific  axes  in  order  to  obtain  an  orientation  are  the  Euler 
angles.  There  are  multiple  combinations  of  angles  and  axes  that  yield  the  same  final  orientation,  so  in  this 
sense  the  “Euler  angles”  are  not  unique  [26],  [39],  One  common  representation,  a  3-1-3  sequence,  is 
shown  in  Figure  4.1  on  the  next  page.  In  this  sequence,  the  first  rotation  is  through  angle  \|/  about  the 
original  “third”  axis,  the  second  rotation  is  of  magnitude  0  about  the  new  “first”  axis,  and  the  final  rotation 
is  through  <p  about  the  new  “third”  axis.  In  this  figure,  the  upper-case  “E”  vectors  are  the  inertial 
directions,  while  the  lower-case  “e”  vectors  are  those  attached  to  the  body  frame.  Tying  this 
representation  back  the  direction  cosines  discussed  earlier,  the  direction  cosine  matrix  (DCM)  that 
transforms  the  inertial  vectors  into  the  rotated  frame,  in  terms  of  the  Euler  angles  defined  here,  is  [40] 

cos  <p  cos  y/  -  sin  0  cos  6  sin  y/  cos  (f>  cos  yr  +  sin  <p  cos  6  cos  yr  sin  <p  sin  6 

DCM  =  -sin0cos^-cos0cos#sin^  -sin0sin^H-cos0cos0sin^  cos^sin#  .  (4.1) 

sin  0 sin  yr  —  sindcosys  cos 9 

As  is  well-documented  in  the  literature,  however,  the  large  computational  burden  associated  with  the  use 
of  this  three  parameter  representation,  as  well  as  the  fact  that  for  some  rotations  the  angles  become 
undefined,  often  makes  them  unsuitable  or  undesirable  for  real-world  problems  [15],  [25],  [36],  [35],  [38], 
[41].  This  problem  with  singularities  is  often  referred  to  as  the  “gimbal  lock”  problem  when  working  with 
gimbaled  inertial  navigation  systems,  but  also  causes  computational  difficulties  in  mathematical 
algorithms  [26].  For  example,  with  the  3-1-3  sequence  defined  here,  it  is  clear  that  if  rotation  of  the  rocket 
causes  the  body  frame  “third”  axis  to  become  aligned  with  the  reference  frame  “third”  axis,  then  the  angle 
<p  becomes  undefined,  as  there  is  no  longer  a  unique  line  of  intersection  between  the  planes  defined  by  the 
“first”  and  “second”  axes  of  each  frame.  At  that  moment,  and  for  as  long  as  the  two  “third”  axes  remain 
aligned,  the  ability  to  distinguish  one  of  the  three  degrees  of  freedom  is  lost.  In  fact,  all  known  three- 


15 


parameter  methods  display  singularity  problems  at  certain  orientations  [26].  Since  a  highly  dynamic 
sounding  rocket  can  be  expected  to  transition  through  many  possible  orientations,  an  Euler  angle-based 
algorithm  is  likely  to  encounter  such  difficulties  [34].  Therefore  Euler  angles,  or  other  three  parameter 
approaches,  are  ill-suited  parameterizations  for  this  problem  [13]. 


Figure  4.1:  Classic  Euler  Rotations  of  a  Rigid  Body  (After  Figure  1.8  in  [40]) 

4.3  Parameterization  Best  Suited  to  This  Problem:  Quaternions 

A  number  of  four  parameter  approaches  have  been  devised  to  avoid  the  singularity  issues  inherent  in  three 
parameter  methods,  where  one  or  more  parameters  become  undefined  at  certain  orientations.  The  basis  for 
these  methods  is  a  set  of  parameters  known  as  quaternions  [26].  Quaternions  are  alternately  known  as 
Euler  symmetric  parameters  [37],  or  less  commonly  as  Cayley-Klein  parameters  [34].  In  the  words  of 
Siouris,  “a  quaternion  is  a  means  of  describing  angular  orientation  with  four  parameters,  the  minimum 
redundancy  that  removes  the  indeterminate  points  of  three-parameter  (Euler  angles)  descriptions  [26].” 
The  foundation  for  the  quaternion  approach  is  Euler’s  theorem  which  states  that  any  finite  rotation  of  a 
rigid  body  can  be  expressed  as  a  rotation  through  some  angle  about  some  fixed  axis  [40],  [42].  In  one 
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interpretation,  three  of  the  four  quaternions  define  the  fixed  rotation  axis  while  the  fourth  gives  the 
magnitude  of  the  angle  of  rotation  about  that  axis  [26],  [34],  [38].  In  this  manner,  the  four  quaternions 
unambiguously  define  the  transformation.  The  price  for  this  lack  of  ambiguity  is  extra  complexity  in  the 
form  of  one  redundant  parameter,  which  in  turn  requires  one  constraint.  Another  drawback  is  that 
quaternions  present  no  obvious  physical  interpretation  of  the  rotation  [40].  The  benefits  are  the  absence  of 
singularities,  an  efficiency  of  computation  relative  to  that  required  for  direction  cosines  or  an  Euler  angle 
approach,  and  greater  robustness  with  respect  to  numerical  round  off  errors  [37],  [38],  [40],  [43]. 

Given  these  advantages  associated  with  a  quaternion  parameterization,  this  research  effort  will  focus  on 
representing  sounding  rocket  attitude  using  quaternions.  Since  these  will  be  an  integral  part  of  the 
development,  it  is  important  to  understand  what  they  are  and  their  relevant  characteristics.  Hamilton  is 
credited  with  inventing  quaternions  in  1843.  They  are  described  as  hyper-complex  numbers  of  rank  4, 
composed  of  a  scalar  part  and  a  three-dimensional  vector  part  [38].  A  quaternion  q  may  be  expressed  as 
[40],  [44] 

9=q  +  $4  ='91  +  ^2+^3+?4-  (4-2) 

where  /, j,  and  k  are  unit  vectors  in  the  hyper-dimensional  plane.  Similar  to  the  familiar  complex  numbers, 
the  conjugate  of  a  quaternion  is  formed  by  simply  taking  the  negative  of  the  imaginary  part 

$ 

q  =  -q + 44  =  -iq i  -  jq-i  -  ^3  +  <24.  (4-3) 

Depending  on  the  author,  the  index  for  the  elements  differs.  For  instance,  Kuipers  and  Siouris  place  the 
scalar  part  first  in  their  notation  and  use  the  index  zero  [26],  [38], 

q  =  +  «  =  +  iql  +  jq2  +  kq3  (4'4) 

and  correspondingly,  the  conjugate  is 

jjf 

q  =q0-<{=qQ-iqrjq1-kch).  (4.5) 

Regardless  of  the  ordering  and  indexing  of  the  elements,  these  notations  represent  equivalent  concepts. 
This  variation  in  notation  causes  no  real  problems,  but  when  using  expressions  for  the  time  derivative  of 
the  quaternion,  etc.,  from  the  literature,  care  must  be  taken  to  be  cognizant  of  the  notation  upon  which  they 
are  based. 

In  addition  to  inventing  the  quaternion  itself,  Hamilton  also  developed  the  now  well-established  rule 

.2  .2  ,2  ...  , 
i  =  j  =  k  =  ijk  =  -1 

ij=~ji=k 
jk  =  -kj  =  i 
ki  =  -ik  =  j 


(4.6) 


for  dealing  with  the  operations  on  the  vector  part  of  the  quaternion  [38],  [44],  Theoretically  there  exist 
hyper-complex  numbers  of  even  higher  rank,  but  as  pointed  out  by  Kuipers,  few  applications  have  been 
found  for  them.  For  the  three-dimensional  orientation  problem,  numbers  of  rank  4,  known  as  quaternions 
are  well  suited.  It  is  now  generally  acknowledged  that  the  principle  use  for  quaternions  is  as  a  rotation 
operator  to  accomplish  three-dimensional  rotations  [38].  Indeed,  given  the  structure  defined  in  equations 
(4.2)  and  (4.4)  above,  it  is  clear  that  traditional  three-dimensional  vectors  may  be  represented  as 
quaternions  with  a  zero  scalar  part. 

Having  established  the  above  definition  of  quaternions,  it  is  helpful  to  understand  some  of  their  relevant 
properties.  Many  authors’  works  on  dynamics  and  attitude  determination  have  included  at  least  a 
summary  of  quaternions  [26],  [37],  [40],  [43]  and  Kuipers  has  written  an  entire  text  on  the  quaternion  as  a 
rotation  operator  [38].  Here  I  will  summarize  the  pertinent  results  that  will  be  used  in  the  later 
development  of  the  attitude  determination  algorithm.  A  more  rigorous  development  may  be  found  in  one 
of  the  works  referenced  above. 

Mathematically,  the  behavior  of  quaternions  is  similar  in  many  ways  to  the  familiar  complex  numbers. 
Perhaps  the  greatest  difference,  and  a  restriction  always  to  be  mindful  of,  is  that  quaternions  are  generally 
not  commutative  under  multiplication  as  is  evident  from  equation  (4.6)  above.  Intuitively  this  makes 
sense,  given  that  quaternions  are  used  as  rotation  operators  and  rotations  must  be  done  in  a  specific  order. 
In  fact,  except  for  certain  special  cases,  changing  the  order  of  a  sequence  of  rotations  changes  the  end 
result.  Additionally,  a  quaternion  can  be  represented  by  a  rotation  matrix,  which  is  not  commutative.  So, 
while  this  result  is  not  surprising,  it  is  one  to  remain  aware  of  while  manipulating  quaternions.  Rotation 
matrices  representing  incremental  rotations  are  one  special  case  that  is  an  exception  to  this  rule.  These 
would  be  rotations  with  an  angle  of  rotation,  0,  such  that  we  may  consider  sin  0  ~  0  and  cos  0-1.  For 
these  incremental  rotations,  the  rotation  matrices  do  indeed  commute  under  multiplication  [38].  Similarly, 
as  is  easily  demonstrated,  the  quaternions  for  this  special  case  also  commute  under  multiplication. 

As  stated  earlier,  the  use  of  quaternions  to  represent  rotations  is  based  on  Euler’s  idea  that  any  rotation  can 
be  defined  as  a  vector  about  which  the  rotation  occurs  and  an  angle  defining  the  magnitude  of  the  rotation. 
While  the  detailed  development  is  beyond  the  scope  of  this  work,  the  transformation  of  a  vector  u  that 
corresponds  to  multiplication  by  rotation  matrix  A, 

u'=  An ,  (4.7) 

is  accomplished  using  quaternion  algebra  as 
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where  q *  is  the  conjugate  of  q  [44].  In  a  matrix  format,  this  quaternion  representation  can  be  written  as 
[38] 

u’=Gu  (4.9) 


which  when  expanded  yields 

r  -i  T  2  2  lr  i 

u (2^4  -l  +  2qj  )  2^4^)  (2^^— 2^4^) 

“2'  =  (29^2  _2^3)  (2?4 -l  +  2^2>  (292«3  +  294«i)  m2  (4l°) 

_“3j  (2^3+ 2^2 )  (2q2q3-2q4q{)  (294-1  +  243)  [M3_ 

or  alternatively,  using  a  different  grouping  of  terms,  in  the  form  [40] 

r-,f2222  1r~i 

w*  (^1  *“^2  ~^3  +^4^  2(^j^2+^3^4)  2(^qfj -^2^4)  «1 

*  2  2  2  2  ^ 
u2'  =  2(^2  -«3?4)  (-?1  +?2  “^3 +^4)  2(^4 +^3)  “2  •  (411) 

.“3 ’ J  2(<7j<73  +  ^2^4 )  2(-gjg4  +^2^3)  (-^i2  -q\+ql+qA^  LM3 . 


It  should  be  noted  that  this  operator  is  interpreted  as  a  frame  rotation,  representing  the  same  vector  in  the 
rotated  frame.  The  corresponding  vector  rotation  operator,  which  represents  the  rotated  vector  in  the 
original  frame,  is  simply  [38] 

u'=quq*.  (4.12) 

Since  rotation  matrices  are  orthogonal,  the  corresponding  matrix  for  this  quaternion  operator  is  simply  the 
transpose  of  those  shown  above,  or  QT.  As  noted  by  Kuipers,  “A  frame  rotation  through  a  certain  angle  is 
entirely  equivalent  to  a  point  rotation  (about  the  same  axis)  but  through  the  negative  of  that  angle  [38],” 


Viewing  the  quaternion  as  a  rotation  operator,  a  convenient  representation  for  a  quaternion,  q ,  is  [38] 

a  a 

q-  cos —  +  ksin —  (4.13) 

2  2 

where  the  rotation  is  about  an  axis  along  the  vector  k  and  the  magnitude  of  the  rotation  is  a.  From  this 
expression  it  is  clear  that  in  order  for  q  to  represent  a  pure  rotation,  with  no  change  in  magnitude,  k  must 
be  a  unit  vector  so  that  the  overall  magnitude  of  q  is  one.  Representing  the  unit  quaternion  q  using  the 
notation  from  equation  (4.2), 

q  =  iqx  +  jq2  +  kqi+q4=q  +  q4  (4.14) 

and  setting  this  equal  to  (4.13)  and  rearranging,  we  see  that  the  angle  of  rotation,  a,  represented  by  the  unit 
quaternion  q  is  [38] 

a  =  2  arccos(#4 ) .  (4.15) 

about  an  axis  in  the  direction  of  q. 
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As  described  earlier,  the  use  of  quaternions  requires  one  constraint  since  there  are  four  parameters  and 
only  three  degrees  of  freedom  in  the  attitude  determination  problem.  As  noted  in  the  preceding  paragraph, 
when  using  quaternions  as  a  rotation  operator,  it  is  necessary  that  the  norm  must  be  unity,  and  this 
provides  the  constraint.  To  verify  that  q  is  indeed  a  unit  quaternion,  the  norm  may  be  calculated  as  [40] 

W(?)  =  ^l2+42+«3+<74  •  (416> 

and  the  constraint  may  therefore  be  viewed  as 

nr,  .  I  2,  2.2.2  2,  2^  2j  2  ,  .~ 

M(q)  =  ^ql  +92+?3+94  =q\  +  <72  +  q3  +«4  = 1  •  <417) 

In  the  case  where  q  is  indeed  a  unit  quaternion,  an  additional  useful  property  is  that  its  inverse  is  simply  its 
conjugate  [38], 

q~l=q*.  (4-18) 


As  alluded  to  earlier,  one  principle  advantage  of  quaternions  over  rotation  matrices  is  an  economy  of 
computation  when  doing  successive  rotations.  For  the  case  of  two  rotations  of  a  three-dimensional  vector, 
the  individual  3x3  rotation  matrices  may  be  multiplied  together  to  obtain  the  matrix  for  the  combined 
rotation.  This  requires  27  multiplications.  In  the  case  of  quaternions  operating  on  a  vector  u,  in 
accordance  with  equation  (4.8)  above, 

“suppose  that  p  and  q  are  unit  quaternions  which  define  the  quaternion  rotation  operators 


* 

L  (u)  =  p  up  andL_(u)  =  <7  u q.  (4.19) 

p  H 

Then  the  quaternion  product  pq  defines  a  quaternion  rotation  operator  Lpq  which 
represents  a  sequence  of  operators,  Lp  followed  by  Lq.  The  axis  and  the  angle  of  rotation 
of  the  composite  rotation  operator  are  those  represented  by  the  quaternion  product  pq 
[38].” 

The  quaternion  product  pq  may  be  expressed  as  [38],  [44] 


m  =  (P4?4  -q-p  +  zvi  +  fyp+pxq)  (4-2°) 

or  in  its  expanded  form  as  [38],  [44] 

pq  =  -  p242  -  P3<?3  +  P^qA ) 

+  »'(P194  +  P2<?3  -  P^2  +  />4?l) 

+  j(~P\q3  +  />2?4  +  p3ql  +  p4q2> 

+  k(pxq2  -  p2q{  +  p?>qA  +  pAq3) 

In  addition  to  having  no  singularities,  and  the  advantage  of  being  a  purely  algebraic  operation,  this  results 
in  a  reduction  in  required  multiplications  from  27  to  16  for  each  combined  rotation  [38],  [40].  For  the 


(4.21) 
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problem  of  attitude  determination,  many  such  successive  rotations  are  required  and  the  result  is  a 
substantial  savings  in  computation  when  using  quaternions  in  place  of  rotation  matrices.  This  further 
bolsters  the  choice  of  quaternions  for  a  real-time  application  such  as  that  envisioned  here. 


Since  this  research  effort  will  use  quaternions  to  represent  the  attitude  of  the  vehicle,  the  quaternion  rates 
will  need  to  be  related  to  the  body  attitude  rates.  Therefore,  another  critical  construct  for  the  development 
of  the  attitude  determination  algorithm  is  the  expression  for  the  time  derivative  of  the  quaternion.  While 
the  development  of  this  expression  can  follow  several  approaches  [26],  [40],  [45],  Chobotov  presents  a 
convenient  representation  that  will  be  used  in  this  work  [40], 


d  1  4 

-(V4>  '  ~hqtjmi  ' 

dt  2 

Here  qy  are  the  components  of  the  four  dimensional  orthogonal  rotation  matrix 


(4.22) 
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(4.23) 


and  cot  are  the  body  rates  as  measured  in  the  reference  frame.  Expanding  equation  (4.22)  yields 
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(4.24) 


Additionally,  as  Chobotov  points  out,  “since  the  terms  are  linear  in  a>h  qj2i  co3 ,  and  also  in  this  equation 
can  alternatively  be  written  as  [40] 
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Finally,  it  will  prove  necessary  to  relate  a  quaternion  to  a  known  set  of  Euler  angles.  This  requires  the 
derivation  of  an  expression  for  each  of  the  quaternion  components.  This  may  be  approached  from  the 
perspective  of  describing  each  Euler  angle  as  a  rotation  about  the  appropriate  axis  by  the  magnitude  of  the 
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Euler  angle  of  interest.  This  allows  each  in  the  3-1-3  sequence  to  be  represented  according  to  equation 
(4.1 1)  as  a  rotation  quaternion  yielding 


yr  *  yr 
qw  =cos— +  *sin  — 
r  2  2 

e  „  e 

Qq  =  cos—  +  /sin  —  . 

<t> ,  r  .  <P 

=  cos  — +  A:  sin  — 
$22 


(4.26) 


These  may  then  be  combined  as  three  subsequent  rotations  using  equation  (4,20)  or  (4.21)  above.  After 
the  quaternion  multiplications  are  carried  out  according  to  the  rules  in  equation  (4.6),  and  terms  are 
combined,  the  following  relationships  emerge  allowing  the  calculation  of  quaternion  components  given 
the  Euler  angles: 


yr  0  <j>  yr  0  <p 

qx  -  cos — sin— cos— +  sin — sin— sin  — 

1  2  2  2  2  2  2 

w  0  <p  yf  6  <p 

q0  -  -  cos — sin— sin  — +  sin — sin— cos— 
2  2  2  2  2  2 

yr  6  0  yr  0  (j> 

q%  -  cos — cos— sin  —  +  sin — cos— cos— 

^  2  2  2  2  2  2 

yr  6  (p  yr  6  <p 

qA  =cos — cos— cos - sin — cos— sin  — 

^  2  2  2  2  2  2 


(4.27) 


Having  summarized  the  advantages  that  make  quaternions  the  desired  parameter  for  this  problem,  and 
their  characteristics  that  will  be  important  for  this  development,  the  next  logical  step  is  an  examination  of 
the  dynamics  involved  in  the  attitude  motion  of  a  sounding  rocket. 
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5.0  Rocket  Rotational  Dynamics 

5.1  Introduction 

As  laid  out  in  the  last  chapter,  quaternions  present  a  preferred  method  for  describing  the  orientation  of  an 
object  in  space.  A  time  history  of  the  quaternion  components  will  provide  a  picture  of  the  three- 
dimensional  rotational  motion  of  the  object,  while  at  each  time  step,  the  quaternion  rates  can  be  related  to 
the  rotational  rates  measured  in  the  body  frame.  As  part  of  the  development  of  an  accurate  attitude 
solution,  a  basic  understanding  of  the  rotational  dynamics  of  a  rocket  is  helpful. 

5.2  Basics  of  Rocket  Rotational  Motion 

Even  a  rudimentary  look  at  the  mechanisms  that  drive  rocket  rotational  motion  quickly  becomes  complex. 
When  looking  at  the  entire  flight  regime,  the  launch  and  ascent  phases  are  especially  troublesome  as  they 
take  place  principally  within  the  sensible  atmosphere.  This  presents  a  number  of  difficulties  when 
attempting  to  develop  an  analytical  expression  to  describe  the  motion  of  a  rocket  as  the  result  of  a 
summation  of  forces  and  moments  acting  on  the  rocket.  Large  portions  of  major  works  have  been  devoted 
to  such  studies  [29],  [46],  [47],  and  yet  there  is  no  straightforward  method  to  predict  rocket  motion  beyond 
a  gross  sense.  A  rigorous,  analytical  approach  depends  heavily  on  the  physical  characteristics  of  the 
specific  vehicle  and  on  the  atmospherics  present  at  the  time  of  launch.  In  describing  drag  and  lift,  two  of 
the  most  significant  forces  impacting  rocket  motion,  Feodosiev  and  Siniarev  point  out  that, 

The  mechanism  of  formation  of  aerodynamic  forces  is  quite  complex.  Determination  of 
D  and  L  forces  by  computation,  even  in  the  simplest  cases,  often  constitutes  an  insoluble 
problem.  Therefore,  currently,  the  determination  of  aerodynamic  forces  (values  of  CD 
and  Ci)  is  done  by  approximate  calculations  which  are  used  as  a  guide  and  which  later  on 
are  corrected  by  wind  tunnel  tests  [29]. 

Since  their  work,  large  six  degree  of  freedom  computer  models  have  been  developed  to  help  predict  such 
motion,  but  they  are  very  computationally  intensive  and  require  the  input  of  many  parameters  describing 
both  the  aforementioned  launch  conditions  and  vehicle  parameters.  Even  with  the  availability  of  such  a 
modeling  program,  complications  exist  due  to  imperfections  in  manufacture  and  assembly  [29],  [47], 
resulting  in  a  difference  between  the  actual  rocket  parameters  and  those  entered  into  the  program.  Adding 
to  the  difficulty  of  analytically  predicting  motion,  is  the  fact  that  the  motion  itself  is  not  simple.  For 
instance,  “an  error  in  alignment  of  the  maximum  moment  of  inertia  with  the  symmetric  body  axis,  about 
which  the  spacecraft  is  spun,  is  common  and  produces  a  coning  of  the  spin  axis  about  the  net  angular 
momentum  vector  [39].”  Additionally,  many  sounding  rocket  vehicles  use  a  combination  of  fin  and  spin 
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stabilization.  Among  other  things,  this  leads  to  a  “coupling”  of  the  motion  in  the  pitch  and  yaw  axes, 
especially  while  still  in  the  atmosphere.  As  described  by  Feodosiev  and  Siniarev,  “If  the  rocket  moves 
with  an  angle  of  attack,  then,  with  the  appearance  of  the  yaw  angle,  the  rocket  will  roll... The  right 
stabilizer  has  additional  velocity,  the  lift  force  on  it  increasing,  whereas  on  the  left  it  decreases.  A  moment 
develops,  rotating  the  rocket  about  its  longitudinal  axis  and  giving  rise  to  rocket  roll  [29].”  In  addition  to 
“cross-coupling”  of  motion  due  to  aerodynamics,  there  is  cross-coupling  associated  with  a  lack  of  mass 
symmetry  combined  with  the  large  angular  momentums  resulting  from  high  spin  rates  [25].  All  of  these 
issues  contribute  to  a  level  of  complexity  that  does  not  lend  itself  to  an  easily  transportable  algorithm  that 
would  be  suitable  for  low-cost  applications  and  across  various  different  platforms. 

Faced  with  the  complexities  of  the  true  dynamics,  other  works  [25],  [31],  [32],  [37],  [45],  [48],  often  treat 
the  rotational  motion  of  rockets  in  a  more  general  and  simplified  sense,  and  that  is  the  approach  that  will 
be  summarized  here.  These  analyses  apply  to  any  spinning  body,  and  the  authors  typically  analyze  motion 
resulting  from  moments  created  about  the  various  body  principle  axes  without  delving  into  the  details  of 
what  generates  those  moments.  Unfortunately,  it  is  often  the  details  that  distinguish  between  the 
theoretical  motion  and  the  true  motion  of  the  rocket. 

From  a  very  simplistic  view,  it  would  at  first  appear  that  sensors  mounted  within  a  rocket  could  measure 
the  rotational  rate  about  each  axis.  These  rates,  in  turn,  could  then  be  integrated  with  respect  to  time  to  get 
a  rotational  angle  at  each  time  step,  thereby  indicating  the  rocket  attitude.  This  approach  is  appealing  as  it 
parallels  the  familiar  “Newton’s  Second  Law  of  Motion”  describing  the  translational  motion.  This 
principle  states  that  the  time  rate  of  change  of  momentum  is  equal  to  the  summation  of  forces  applied  to 
the  object.  The  more  well-known  version  being  “the  sum  of  the  forces  is  equal  to  mass  times 
acceleration,”  which  is  the  result  one  gets  by  assuming  a  constant  mass.  For  translational  motion,  the 
acceleration  obtained  by  the  force  summation  is  then  integrated  once  to  get  velocity,  and  then  again  to  get 
position.  The  not  so  obvious  requirement  is  that  Newton’s  laws  only  hold  true  in  an  inertial  reference 
frame.  So,  while  there  is  a  rotational  corollary  to  the  familiar  Newton’s  Second  Law,  the  sum  of  the 
moments  is  equal  to  the  time  rate  of  change  of  angular  momentum,  and  the  angular  acceleration  and 
angular  rates  can  be  integrated  to  get  angular  orientation,  this  also  must  happen  with  respect  to  an  inertial 
reference  frame.  As  described  in  Chapter  3,  the  body  frame  is  not  an  inertial  frame.  Therefore,  as  pointed 
out  by  many  authors  [32],  [38],  [40],  the  rates  expressed  in  the  body  frame  are  not  suitable  for  integration. 
If,  however,  the  rates  can  be  determined  in  a  sufficiently  inertial  frame,  they  can  be  integrated  with  respect 
to  time  to  get  rotational  orientation  with  respect  to  the  inertial  frame.  As  will  be  shown  in  a  later  section, 
this  will  be  an  integral  concept  in  the  simulation  phase  of  the  work  accomplished  here. 
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In  his  discussion  of  what  presents  a  suitable  set  of  coordinates  for  integration,  and  therefore  for  predicting 
attitude,  Meirovitch  points  out  that 

Nor  do  the  direction  cosines  /#  meet  this  need,  because  they  are  not  independent,  which 
rules  them  out  from  qualifying  as  a  set  of  generalized  coordinates.  To  describe  the 
orientation  of  a  rigid  body  in  space  we  need  three  independent  coordinates.  Such  a  set  of 
coordinates  is  not  necessarily  unique,  and... A  set  which  enjoys  wide  acceptance  consists 
of  Euler’s  Angles .  These  are  three  successive  angular  displacements  which  can 
adequately  carry  out  the  transformation  from  one  Cartesian  system  of  axes  to  another, 
although  the  rotations  are  not  about  three  orthogonal  axes.  Moreover,  the  three 
components  of  the  body  angular  velocity  can  be  expressed  in  terms  of  Euler’s  angles  and 
their  time  derivatives  [32]. 

So,  to  summarize,  the  body  rates  cannot  be  integrated,  but  the  Euler  angle  rates  may,  since  they  are  inertial 
quantities.  Of  course,  care  must  be  taken  to  account  for  the  singularities  associated  with  using  Euler 
angles,  as  described  in  Chapter  4. 

The  overarching  equation  governing  the  rotational  motion  of  rigid  bodies  is  indeed  Newton’s  Second  Law 
of  Rotational  Motion... the  sum  of  the  moments  is  equal  to  the  time  rate  of  change  of  angular  momentum. 
The  more  familiar  version  of  this  law  is  “the  sum  of  the  moments  is  equal  to  rotational  moment  of  inertia 
times  the  angular  acceleration.”  This  simplification  is  based  on  the  assumption  that  the  moment  of  inertia 
does  not  change  with  time.  An  expansion  of  this  law,  taking  into  account  all  three  axes,  and  substituting 
relations  for  the  rates  measured  in  the  body  frame,  yields  the  familiar  Euler’s  Equations  of  Rotational 
Motion  [30] 

A/j  =  Ad ^  +  (C  -  B)co 2^ 

M2  =  Bd)2  +(A-C)0)^0)^  (5.1) 

M  3  =  Cd +  (/?-  A)o^co 2 

where 

=  moment  about  the  body  first  axis, 

M  2  =  moment  about  the  body  second  axis, 

M  ^  =  moment  about  the  body  third  axis, 

6>y  =  rotational  rate  about  the  body  first  axis, 

6) 2  =  rotational  rate  about  the  body  second  axis. 


0)^  =  rotational  rate  about  the  body  third  axis, 
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A  =  principle  moment  of  inertia  about  the  body  first  axis, 

B  =  principle  moment  of  inertia  about  the  body  second  axis, 

C  =  principle  moment  of  inertia  about  the  body  third  axis. 

In  turn,  the  body  rates  may  be  conveniently  related  to  the  inertial  Euler  angle  rates  using  the  “Euler  angle 
rate  equations.”  There  is  no  universally  accepted  set  of  Euler  angles  or  notation  for  them,  but  using  the 

angle  sequence  depicted  in  Figure  4.1,  and  in  the  notation  of  Chobotov  [40]  and  Kaplan  [27],  these 

equations  are 

0)y  =  y/sin#sin0  +  #cos0 

#2  =  ^rsin#cos^“#sin0  (5.2) 

co^  —  <f>  +  \(/  cos  6 

where 

c a j  =  rotational  rate  about  the  body  first  axis, 

0) 2  =  rotational  rate  about  the  body  second  axis, 

CD^  =  rotational  rate  about  the  body  third  axis, 
if/  =  clockwise  angle  about  third  axis, 

6  -  clockwise  angle  about  “new”  first  axis, 

(j>  -  clockwise  angle  about  “new”  third  axis. 


Since  the  Euler  angles  do  not  constitute  an  orthogonal  transformation  [40],  the  inverse  transformation 
relating  the  Euler  angle  rates  as  a  function  of  the  body  rates  is  not  simply  the  transpose  of  the 
transformation  matrix  derived  from  (5.2)  above,  but  must  be  solved  for.  The  resulting  relationship  in  a 


matrix  form  is  [27],  [40] 


# 


1 

sin# 


sirup 

-  sin  (p  cos# 
^  cos (j) sin# 


COS  (f) 

-  cos  (p  cos# 

-  sin  (j>  sin# 


0 

sin 

0 


^  0)^  ^ 

^2 
A*i) 


(5.3) 


where 

if/  -  clockwise  angle  about  third  axis, 

#  =  clockwise  angle  about  “new”  first  axis, 

<p  -  clockwise  angle  about  “new”  third  axis, 

(o^  =  rotational  rate  about  the  body  first  axis, 
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co j  =  rotational  rate  about  the  body  second  axis, 
a =  rotational  rate  about  the  body  third  axis. 

The  relationships  between  the  body  rates  and  the  Euler  angle  rates  given  in  equations  (5.2)  and  (5.3) 
above,  as  well  as  the  relationship  between  body  rates  and  quaternion  rates  given  in  equation  (4.22)  of  the 
previous  chapter,  provide  convenient  methods  to  express  rotational  motion  in  the  body  and  inertial  frames. 
Each  will  be  used  in  the  development  laid  out  in  a  later  chapter. 

5.3  Simplified  Model  of  Rocket  Motion 

A  number  of  goals  associated  with  this  effort  drive  a  move  to  a  simplified  model  of  the  rocket  dynamics. 
First,  it  is  desirable  that  the  model  be  applicable  to  various,  if  not  all,  rockets  with  little  re-derivation 
required.  Additionally,  in  keeping  with  the  desired  “low-cost”  nature  of  the  solution,  the  procurement  and 
hosting  of  complicated  six-degree  of  freedom  models  is  not  realistic.  Even  the  “simplified”  dynamic 
equations  summarized  in  the  last  section  require  accurate  prediction  of  moments  acting  on  the  rocket 
throughout  its  flight,  as  well  as  accurate  measurement  of  the  physical  parameters  that  make  up  the 
moments  of  inertia.  While  these  efforts  are  routinely  accomplished  for  very  expensive  missions,  they  do 
not  lend  themselves  to  a  “low-cost”  effort  such  as  that  undertaken  by  most  universities. 

First,  it  is  important  to  realize  where  dynamic  models  will  be  required  for  this  effort  and  what  level  of 
fidelity  is  necessary.  During  development  of  the  algorithm  and  subsequent  testing,  rocket  motion  will  be 
simulated  to  provide  a  “truth”  reference.  Noisy  measurements  of  this  truth  reference  will  be  simulated  as 
well.  The  algorithm  will  operate  on  these  in  an  attempt  to  recover  the  uncorrupted  motion.  For  this 
simulation  phase,  the  best  case  would  be  a  situation  where  you  could  “fly”  the  rocket  through  a  number  of 
perturbing  moments  and  come  up  with  the  exact  expected  motion  of  the  vehicle.  For  the  reasons  discussed 
in  the  previous  section,  a  model  of  this  fidelity  is  not  realistic,  and  for  simulation  purposes  is  not 
necessary.  To  develop  and  test  the  algorithm,  motion  that  is  typical  of  real  motion,  having  similar 
complexity,  will  be  simulated.  Second,  several  of  the  filtering  techniques  to  be  examined  require  a 
dynamic  model  to  propagate  states  forward  in  time.  Fortunately,  due  to  the  small  time  steps  used  and  the 
fact  that  the  model  is  embedded  within  the  filter,  this  model  does  not  need  to  be  very  high  fidelity,  as  will 
be  demonstrated  in  a  later  section. 

Given  these  inherent  limitations  and  the  requirements  for  this  effort,  the  following  simplified  dynamic 
model  is  proposed.  This  model  is  used  in  the  filtering  efforts  as  a  relatively  low  fidelity,  but  adequate, 
model  of  the  rocket  rotational  motion.  It  has  the  benefit  of  requiring  no  measurement  or  derivation  of 
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rocket  physical  parameters,  and  is  therefore  transportable  between  vehicles.  In  fact,  this  simplified  model 
should  allow  this  algorithm  to  be  applied  to  the  attitude  determination  of  any  rotating  object,  as  long  as  the 
filter  is  tuned  to  the  expected  motion.  As  presented  by  Marins  [II],  the  simplest  dynamic  model  for 
generating  angular  rates  is  one  based  on  rates  driven  by  white  noise,  in  which  the  derivative  of  the  rates  is 
a  function  of  the  rates  themselves.  As  described  in  the  previous  paragraph,  this  model  is  to  be  embedded 
in  the  filter  and  therefore  does  not  need  to  be  of  high  fidelity.  Implicit  in  its  use,  however,  is  the 
assumption  that  the  inertia  of  the  rocket  prevents  its  motion  from  changing  radically  over  the  duration  of 
one  filter  time  step.  This  is  necessary  because  the  model  represents  the  new  rotational  rate  as  the  previous 
rate  perturbed  by  a  small,  random,  acceleration  scaled  by  the  time  constant  xr.  Longer  time  steps  may 
indeed  require  a  higher  fidelity  dynamic  model  for  the  propagation  of  states  within  the  filter.  For  this 
application,  however,  at  small  time  steps  relative  to  the  motion  of  the  rocket,  this  rudimentary  model  is 
adequate.  The  dynamic  model  for  the  angular  rates  may  be  represented  by  Figure  5.1  below.  In  this 


Figure  5.1:  White  Noise  Driven  Angular  Rates  (After  Figure  3.1  in  [11]) 

figure, 0)  is  a  3x1  vector  of  angular  rates  {ob  ©2,  and  ©3  in  the  body  frame,  wr  is  a  3x1  vector  of  white  noise 
components  generating  (ou  o)2,  and  <o3,  and  xr  is  a  3x1  vector  of  time  constants  corresponding  to  o)b  ©2, 
and  ©3.  The  determination  of  both  the  time  step  and  the  time  constants  will  be  elaborated  upon  in  Chapter 
10.  Marins  et  al.  [12],  used  this  model  successfully  with  respect  to  a  low-dynamics  submersible  vehicle. 
It  will  be  demonstrated  in  a  later  section  that,  with  proper  scaling  through  the  time  constants  and 
sufficiently  small  time  steps,  it  also  functions  adequately  for  the  much  higher  rotation  rates  seen  in  the 
case  of  rocket  motion. 

Having  examined  some  of  the  characteristics  that  make  rocket  motion  complex,  as  well  as  surveying  the 
equations  that  help  describe  this  motion,  it  is  now  necessary  to  investigate  how  the  motion  might  be 
monitored,  or  measured,  given  the  constraints  of  this  effort.  These  measurements  will  provide  the  raw 
material  from  which  the  algorithm  determines  a  best  estimate  of  the  rocket’s  rotational  motion. 
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6.0  Attitude  Sensing 

6.1  Introduction 

With  a  basic  understanding  of  the  rotational  motion  of  a  rocket  and  its  payload,  the  means  of  measuring 
the  motion  can  now  be  examined.  While  a  perfect  dynamic  model  would  allow  a  precise  projection  of  the 
motion,  as  pointed  out  in  Chapter  5,  such  a  model  does  not  exist.  Therefore,  any  algorithm  that  is 
designed  to  provide  a  best  estimate  of  rocket  attitude  must  be  supplied  with  measurements  that  can  be 
combined  and  filtered  to  yield  an  accurate  solution.  This  chapter  will  evaluate  the  types  of  measurements 
available,  and  present  those  selected  as  the  best  choice  for  this  effort. 

6.2  Common  Attitude  Sensors 

A  survey  of  attitude  sensors  reveals  that  there  are  many  possible  sensors,  but  few  that  are  commonly  used 
in  practice.  From  this  reduced  set  of  sensors,  an  even  smaller  set  is  best- suited  for  use  on  a  sounding 
rocket  platform.  A  summary  of  this  survey  follows. 

Chobotov  provides  a  succinct  list  of  the  most  commonly  used  attitude  sensors  to  include  Earth  horizon, 
Sun,  star  and  inertial  measuring  units  (IMUs).  He  goes  on  to  point  out  that  magnetometers  and  radio 
interferometers  are  also  used  [40].  The  Global  Positioning  System  (GPS),  long  known  as  an  accurate 
source  of  position  information,  is  also  being  used  for  attitude  determination  in  a  limited  number  of 
applications  [49].  This  list  is  based  primarily  on  satellite  attitude  sensing,  although  each  entry  might  also 
find  its  place  on  a  sounding  rocket  mission,  if  sufficient  budget  is  available.  Each  method/sensor  has  its 
benefits  and  drawbacks  and  some  are  not  especially  well-suited  to  the  sounding  rocket  mission.  Combined 
with  the  cost  and  facility  constraints  associated  with  the  goal  of  this  research,  many  can  be  eliminated, 
leaving  a  few  that  present  themselves  as  likely  candidates. 

Earth  horizon  sensors  come  in  scanning  and  non-scanning  varieties  that  can  have  accuracies  down  to  the 
tenth  of  a  degree  range  [40],  [50].  Merely  detecting  the  presence  of  the  Earth  is  insufficient  [51],  so  they 
are  typically  designed  to  detect  the  Earth  horizon  and'  then  determine  a  local  vertical  direction.  Often  the 
results  from  two  sensors  are  averaged  to  minimize  errors  [27].  Interpretation  of  horizon  sensor  data  can  be 
complicated  by  the  need  to  correct  for  Earth  oblateness  effects  to  achieve  maximum  accuracy  [49]. 
Probably  the  most  significant  drawback  for  this  application  is  the  fact  that  their  accuracy  is  limited  by 
horizon  definition  and  they  degrade  at  lower  altitudes,  precisely  the  flight  regime  of  sounding  rockets. 
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Sun  sensors  are  arguably  the  most  common  attitude  sensor  employed  on  both  satellites  and  sounding 
rockets,  at  least  for  daytime  launches  [18],  [52].  For  night  launches,  a  Moon  sensor  or  horizon  sensor 
may  be  substituted.  The  Sun  is  typically  treated  as  a  point  source  for  low-altitude  satellite  and  sounding 
rocket  applications,  subtending  approximately  .267  deg  [52]  or  7  x  10'5  steradians  [51]  at  Earth  orbit  about 
the  Sun.  They  come  in  both  analog  and  digital  designs.  Expensive  varieties  can  have  accuracies  as  good 
as  .01  deg,  but  this  is  difficult  to  achieve  [49],  [50].  Smith  reports  that  a  commonly  used  digital  Sun 
sensor  produces  accuracies  on  the  order  of  .5  degrees  [39].  One  significant  advantage,  for  this  effort,  is 
that  Sun  sensors  can  be  designed  and  built  in-house,  by  university  student  programs,  or  other  programs 
with  smaller  budgets.  For  example,  the  university  designed  and  built  Sun  sensor  flown  on  the  Student 
Rocket  Project  4  (SRP-4)  mission  achieved  a  reported  accuracy  of  4  degrees  [53].  It  is  not  clear  how  this 
reported  “accuracy”  is  defined,  but  assuming  “accuracy”  can  be  interpreted  as  the  3a  value,  meaning  99% 
of  the  values  fall  in  this  range,  and  assuming  a  Gaussian  measurement  error  distribution,  the  standard 
deviation  for  this  “accuracy”  could  be  considered  to  be  1.33  degrees,  or  approximately  0.023  radians. 

Star  sensors  are  the  most  accurate  attitude  sensors  available.  While  they  are  very  accurate,  they  have  the 
potential  downside  of  being  blinded  by  the  Sun,  Moon,  or  planets  [49].  Star  sensors  are  most  effective 
when  a  fairly  accurate  estimate  of  attitude  is  already  known,  and  the  star  sensor  data  is  then  used  to  refine 
the  estimate  [18].  Additionally,  they  are  very  complex  and  expensive,  which  eliminates  them  from 
practical  consideration  for  this  research  effort. 

Inertial  Measurement  Units,  or  IMUs,  typically  consist  of  a  collection  of  gyroscopes  and  accelerometers 
and  have  the  unique  ability  to  determine  position  and  attitude  independent  of  outside  sources  of 
information.  Once  given  an  accurate  initialization  consisting  of  its  position  and  attitude,  and  assuming 
perfect  gyros  and  accelerometers,  an  IMU  will  accurately  measure  and  report  both  position  and  attitude. 
This  type  of  unit  has  been  used  successfully  on  many  different  platforms,  the  most  notable  being  manned 
spacecraft,  nuclear  submarines,  and  intercontinental  ballistic  missiles.  Unfortunately,  components  with 
sufficient  accuracy  to  make  IMUs  practical  are  very  expensive.  Even  given  robust  filtering  and  estimating 
techniques,  and  frequent  reinitialization  from  outside  reference  sources,  IMUs  are  impractical  from  a  cost 
and  manufacturing  standpoint  for  a  “low-cost”  application.  Recent  developments  in  solid-state  gyroscopes 
and  accelerometers  may  make  this  a  viable  alternative  in  the  future,  but  at  this  point  in  time,  they  are  not  a 
practical  option  for  the  work  envisioned  here. 

While  the  quality  of  components  needed  to  implement  a  practical  IMU  are  cost  prohibitive,  gyroscopes 
can  be  used  alone  as  attitude  sensors.  The  two  major  categories  are  rate  gyros  that  measure  rotation  rate, 
as  their  name  implies,  and  rate  integrating  gyros  that  output  an  angular  displacement  from  an  initial  value. 
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The  less  expensive  versions  of  these  gyroscopes  have  drift  rates  far  in  excess  of  what  is  acceptable  for  use 
as  part  of  an  IMU,  but  over  short  time  steps,  can  provide  adequate  measurement  of  rotation  rates  to  be 
used  as  inputs  to  a  filtering  algorithm  or  dynamics  model.  Rate  integrating  gyros  are  typically  far  more 
accurate  than  rate  gyros,  but  are  often  far  more  expensive  [54].  A  problem  with  using  gyros  in  any 
attitude  determination  system  where  Kalman-based  filtering  is  used  is  that  the  low  frequency  noise 
component  (also  referred  to  as  bias  or  drift)  violates  the  white  noise  assumption  required  for  Kalman 
filtering.  Although  this  issue  can  sometimes  be  mitigated  by  forming  a  larger  state  vector  augmented  with 
the  gyro  noise,  the  result  is  a  significantly  more  complex  estimator  [41].  Expensive  gyros  exhibit  drift 
rates  as  low  as  .001  deg/hr,  but  recently  developed  solid  state  gyros,  that  would  fit  the  budget  of  most 
“low-cost”  programs,  have  drift  rates  as  high  as  300  deg/hr.  While  these  high  drift  rates  make  their  use 
questionable  for  use  in  a  rate-integrating  sense,  three  inexpensive  rate  gyros  mounted  orthogonally  can 
provide  noisy  measurements  of  the  body  frame  rates  about  each  axis. 

In  addition  to  low  cost  gyros,  an  alternative  low-cost  source  of  rate  measurements  might  be  found  in  the 
recent  development  of  inexpensive  accelerometers.  Again,  these  do  not  have  sufficient  accuracy  for  use  in 
an  IMU  application,  but  they  may  provide  useful  inputs  to  a  filtering  algorithm  such  as  that  envisioned 
here.  Rates  could  possibly  be  obtained  either  through  the  integration  of  the  accelerometer  outputs,  or 
possibly  by  processing  tangential  and  radial  components  of  acceleration  about  each  body  axis. 

Another  commonly  used  sensor  is  the  magnetometer.  A  three-axis  magnetometer  mounted  at  a  known 
orientation  to  the  vehicle  body  frame  can  measure  the  strength  and  direction  of  the  local  magnetic  field 
vector.  This  allows  for  the  determination  of  the  spacecraft  orientation  relative  to  the  magnetic  field. 
Recent  advances  in  modeling  the  Earth’s  magnetic  field  have  improved  the  workability  of  magnetometers 
as  attitude  sensors.  Reeves  reports  an  accuracy  of  approximately  5  degrees  at  an  altitude  of  200  km  [50]. 
Despite  advances  in  field  modeling,  changes  in  the  field  due  to  solar  effects  and  other  perturbations  limit 
the  accuracy  of  these  devices  for  measuring  attitude.  As  with  the  Sun  sensor,  a  significant  advantage  of 
this  type  of  sensor  for  the  work  considered  here  is  that  it  can  be  designed  and  manufactured  in-house  at 
reasonable  cost  by  experimenters  in  a  “low-cost”  environment  [53].  Additional  benefits  of  magnetometers 
are  that  they  are  vector  sensors,  providing  both  the  direction  and  magnitude  of  the  magnetic  field, 
reliability,  light  weight,  low  power  requirements,  wide  temperature  operating  range,  and  they  have  no 
moving  parts  [55].  While  they  are  sometimes  limited  in  satellite  applications  by  the  1/r3  dependence  of 
field  strength  on  distance  from  the  source,  for  relatively  low  altitude  sounding  rocket  missions,  this  is  not 
an  issue.  Additional  concerns  include  the  effects  of  the  magnetic  environment  within  the  vehicle  and 
uncertainties  in  the  magnetic  field  model.  Chobotov  claims  that  “magnetometers  can  provide  directional 
accuracy  to  within  one  degree  of  the  magnetic  field  vector  [40],”  and  Fortescue  and  Stark  report  that 
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accuracy  is  limited  to  approximately  0.5  degrees  [25].  Herbert  claims  an  accuracy  of  .25  degrees  or  better 
[4].  Low-cost  versions  of  this  instrument  most  likely  will  not  achieve  these  accuracies.  The  three-axis 
magnetometer  designed  and  built  by  the  Tokai  University  and  flown  on  Student  Rocket  Project  4  (SRP-4) 
demonstrated  an  accuracy  of  approximately  0.5  degrees  [56].  As  with  the  Sun  sensor,  in  order  to  use  a 
magnetometer  to  determine  attitude,  an  accurate  position  of  the  vehicle  is  required  in  order  to  cross 
reference  the  measured  field  with  that  predicted  by  a  model  [57].  From  a  design  and  implementation 
standpoint,  careful  calibration  must  be  accomplished  taking  into  account  magnetization  due  to  the  rocket 
itself  and  potential  changes  to  the  magnetic  environment  throughout  the  flight  [3],  [4], 

Radio  interferometer  and  GPS  attitude  determination  are  similar  methods  from  a  technology  point  of  view. 
Each  relies  on  the  reception  of  a  known  radio  signal  at  two  antennas  or  at  a  single  antenna  at  two  distinct 
times.  While  these  methods  promise  light  weight  and  low  cost,  Eterno  points  out  a  number  of  challenges 
to  successfully  implementing  them,  among  them  are  that  “accuracy  is  limited  when  used  as  an  attitude 
sensor  by  separation  of  antennas,  the  ability  to  resolve  small  phase  differences,  relatively  long  wavelength 
of  signal  and  multipath  issues  [49].” 

Yet  another  radio-based  method  makes  use  of  directional  antennas  and  signal  strength  to  estimate  attitude. 
The  received  signal  strength  of  the  vehicle  telemetry  signal  at  the  ground  station  is  used 

to  determine  vehicle  aspect  with  respect  to  the  vector  describing  the  position  of  the 
vehicle  as  seen  from  the  ground.  Here  the  variation  of  signal  strength  as  the  vehicle 
precesses  is  used.  This  method  has  also  had  limited  success  since  normally,  the  vehicle 
may  precess  only  a  few  times  in  a  flight  and  the  signal  strength  variation  due  to 
precession  is  partially  masked  by  the  signal  strength  variation  due  to  changes  in  vehicle 
range  throughout  the  flight  and  other  effects  [5]. 

Under  ideal  conditions,  this  type  of  directional  antenna  method  can  produce  accuracies  of  approximately 
.01 -.5  deg  (~1%  of  antenna  beam  width)  [50]. 

In  the  end,  what  is  required  is  a  truly  inertial  system  that  can  be  initialized  and  then  allowed  to  propagate 
the  attitude  without  external  inputs,  or  measurement  of  two  or  more  external  references  at  each  time  of 
interest  from  which  an  unambiguous  estimate  of  the  attitude  at  that  moment  can  be  computed  [27],  [49]. 
Based  on  the  sensor  characteristics  outlined  above,  in  conjunction  with  the  goals  of  this  research,  the 
sensors  best  suited  to  this  application  are  a  Sun  sensor,  a  three-axis  magnetometer,  and  a  set  of  three 
orthogonally  mounted  rate  gyros. 
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6.3  Sensors  Best  Suited  to  This  Problem:  Sun  Sensor,  Magnetometer,  Rate  Gyro 

Recognizing  that  a  precise  Inertial  Measurement  Unit  is  beyond  the  budget  of  many  “low-cost"  research 
efforts,  a  sensor  suite  capable  of  measuring  direction  to  external  references  is  dictated.  A  minimum  of  two 
nonparallel  directions  are  needed  to  uniquely  determine  the  rocket’s  orientation  [27].  Culling  the  available 
sensor  types  and  evaluating  the  current  costs  and  state  of  technology  results  in  a  suite  composed  of  a  Sun 
sensor  and  a  triaxial  magnetometer  to  measure  the  two  reference  directions,  and  inexpensive  rate  gyros  or 
accelerometers  are  a  possibility  to  cost-effectively  provide  rotational  rate  information.  While  these  are  not 
strictly  necessary  for  instantaneous  attitude  determination,  many  filtering  algorithms  use  a  dynamic  model 
to  refine  the  estimate  of  the  attitude.  As  discussed  in  the  section  on  dynamics,  the  angular  rates  determine 
how  the  vehicle  behaves  dynamically,  so  a  measurement  of  these  rates  is  useful.  In  fact,  if  perfect  sensors 
provided  the  real  values  of  the  angular  rates  and  an  accurate  initial  position  was  available,  “integrating  the 
angular  rates  through  Euler  equations  would  lead  us  to  determine  the  orientation  at  each  time  step  [11].” 
While  the  rate  sensors  that  can  be  afforded  here  will  not  be  sufficient  for  such  a  process,  they  may  provide 
adequate  information  to  allow  the  filtering  algorithm  to  further  refine  the  estimates  of  attitude  over  what 
would  be  achieved  using  only  the  vector  sensors. 

The  algorithm  to  be  developed  will  focus  on  the  manipulation  of  data  from  any  two  vector  sensors.  While 
the  discussion  above  has  narrowed  the  field  to  what  would  best  suit  this  particular  application,  the 
algorithm  could  make  use  of  others.  For  instance,  in  the  case  of  a  night  launch,  it  might  be  necessary  to 
substitute  a  star,  Moon  or  a  horizon  sensor  in  place  of  the  Sun  sensor.  Likewise,  if  some  new  rate  sensor 
became  available,  it  could  be  substituted  for  the  rate  gyro.  In  this  sense,  the  algorithm  is  “blind”  to  where 
the  data  comes  from,  merely  requiring  the  measured  values  and  the  quality  of  the  measurements.  One 
additional  constraint  on  the  choice  of  vector  sensors  is  that  a  model  must  be  available  to  provide  values  for 
the  measured  vector  in  the  reference  frame  at  the  times  of  interest. 

The  development  of  the  algorithm  will  proceed  using  a  baseline  of  a  Sun  sensor  with  a  measurement 
accuracy  of  4  degrees,  corresponding  to  a  standard  deviation  of  1.333  degrees  using  the  3a  interpretation 
described  above.  This  corresponds  to  what  was  reported  by  Tokai  University  based  on  their  experience 
from  the  SRP-4  mission  [53].  Based  on  the  information  in  the  previous  section,  performance  at  least  this 
good  should  be  expected.  A  triaxial  magnetometer  with  a  measurement  accuracy  of  10  degrees, 
corresponding  to  a  standard  deviation  of  3.333  degrees  using  the  3a  approach,  will  be  used  as  a  baseline 
for  the  second  vector  sensor.  This,  or  better,  should  be  readily  available  from  a  magnetometer  designed 
and  built  “in-house”  at  a  university  or  similar  program,  considering  the  reported  accuracy  of  the  SRP-4 
magnetometer  at  +/-  0.5  degrees  in  each  axis.  [56].  Given  model  uncertainty  for  the  magnetic  field  and 
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errors  in  the  position  solution  of  the  rocket,  a  standard  deviation  of  10  degrees  should  be  very 
conservative.  Finally,  algorithms  will  be  designed  that  both  use  and  do  not  use  rate  information  such  as 
that  provided  by  the  rate  gyros,  or  accelerometers,  described  above.  This  will  enable  a  comparison  of 
what  benefit,  if  any,  is  gained  relative  to  the  increased  complexity  of  including  the  rate  sensor  in  a  sensor 
suite.  As  a  baseline,  a  rate  sensor  with  a  measurement  accuracy  of  1  revolution  per  minute  (rev/min), 
corresponding  to  a  standard  deviation  of  0.333  rev/min  will  be  used. 

Due  to  the  non-inertial  nature  of  these  sensors,  an  accurate  estimate  of  the  vehicle  position  and  an  accurate 
time  reference  are  necessary  for  determination  of  rocket  attitude.  These  are  required  to  reference  solar 
ephemeris  and  magnetic  field  models  to  determine  the  inertial  reference  vectors  at  the  time  of  the  estimate. 
This  in  turn  enables  the  calculation  of  the  pointing  vector  of  the  rocket,  or  the  attitude.  For  this  work, 
these  position  and  time  references  are  assumed  to  be  available,  either  from  GPS,  RADAR,  or  other 


sources. 
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7.0  Applicable  Sensor  Fusion  Principles 

7.1  Introduction 

In  this  chapter,  I  discuss  several  of  the  more  prominent  methods  for  “fusing”  data  that  may  be  applicable 
to  this  problem.  Herein  I  evaluate  each  and  support  the  choice  that  will  be  used  for  this  research  effort. 
The  term  “sensor  fusion”  is  somewhat  ill-defined  and  can  be  presumed  to  include  many  things.  In  this 
work,  I  distinguish  between  “fusion”  and  “filtering,”  although  some  might  include  filtering  as  a  type  of 
fusion  [58],  [59].  I  will  discuss  “filtering”  methods  as  they  apply  to  the  rocket  attitude  determination 
problem  in  Chapter  8  of  this  work.  In  this  chapter,  I  limit  the  discussion  to  those  methods  that  operate  on 
multiple  noisy  measurements  taken  simultaneously,  as  opposed  to  a  method  that  somehow  analyzes  a  time 
history  of  measurements.  In  the  end,  the  algorithm  developed  for  this  effort  will  employ  techniques  from 
each  group. 

7.2  Sensor  Fusion  Techniques:  Weighted  Averaging,  Error  Checking,  Error  Minimization  / 
Parameter  Optimization 

Among  the  data  fusion  techniques  in  common  use,  some  are  very  simple  while  some  are  more 
sophisticated.  ‘Techniques  to  combine  or  fuse  data  are  drawn  from  a  diverse  set  of  more  traditional 
disciplines,  including  digital  signal  processing,  statistical  estimation,  control  theory,  artificial  intelligence, 
and  classic  numerical  methods  [60].”  The  goal  for  this  work,  with  respect  to  data  fusion,  is  to  survey  the 
methods  available  and  investigate  what,  if  any,  tools  might  be  successfully  applied  to  this  effort. 

Many  of  the  fusion  efforts  described  in  the  literature  are  designed  to  arrive  at  a  better  solution  given 
multiple  sensors  measuring  the  same  quantity  [60],  [61],  [62].  For  example,  if  you  have  two  or  more  Sun 
sensors  and  you  combine  them  in  an  intelligent  way,  you  may  be  able  to  get  a  better  answer  than  you 
would  from  a  single  sensor,  owing  to  the  additional  information  available.  One  common  approach  to  this 
is  a  type  of  weighted  averaging  summarized  by  Perrella  [61]  wherein  the  overall  estimate  is  achieved  by 
adding  the  multiple  measurements  together,  after  suitably  weighting  them  based  on  some  estimate  of  their 
relative  quality.  Typically,  the  “quality”  of  an  estimate  is  measured  by  its  standard  deviation  or  variance. 
Obviously,  it  is  desirable  that  the  standard  deviation  of  the  fused  estimate  be  less  than  that  of  the 
individual  estimates.  Perrella  points  out  that  a  careful  choice  of  weights  often  achieves  this,  while  a  poor 
choice  might  actually  do  more  harm  than  good.  As  an  example  of  this  type  of  fusion,  in  an  extension  of 
the  two  sensor  case  described  by  Perrella,  the  “fused”  estimate  from  three  sensors  is  calculated  as 

*  jterf  -  V  *i  +  » 2  *  *2 +<1  -^i  -ny  <  x3 , 


(7.1) 
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yielding  a  variance  of  the  fused  estimate  determined  by 

o-jused  =  (Wjtf,  )2  +  (W2a2  )2  +  [(1  -Wt-W^o 3  ]2 .  (7.2) 


Clearly,  the  goal  is  to  choose  the  weights  so  as  to  minimize  the  variance  of  the  fused  estimate.  Taking  the 
gradients  of  (7.2)  with  respect  to  the  weights,  setting  them  equal  to  zero,  and  solving  yields  the  following 
expressions  to  calculate  the  weights  Wi,  W2  and  W3  to  minimize  o2fused. 
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W3=l-Wl-W2. 
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(7.5) 


While  such  an  approach  has  potential  benefits  in  an  application  similar  to  that  devised  in  this  effort,  in 
order  to  minimize  complexity  and  cost,  only  one  sensor  is  employed  to  measure  each  quantity  here.  This 
and  other  methods  for  intelligent  synthesis  of  multiple  sensors  warrant  further  investigation  for  an 
application  where  the  budget  allows  such  a  configuration.  It  has  been  shown  that,  “a  statistical  advantage 
is  gained  by  adding  the  N  independent  observations  ...  assuming  the  data  are  combined  in  an  optimal 
manner  [60].”  Fortunately,  he  also  points  out  that  a  statistical  advantage  may  also  be  obtained  by 
intelligently  combining  N  observations,  over  time,  from  an  individual  sensor.  Rather  than  employ 
redundant  sensors,  at  additional  cost  and  system  complexity,  it  is  this  second  approach,  in  the  form  of 
“filtering”  that  is  implemented  in  this  work.  The  details  are  described  in  the  next  chapter  on  filtering. 


In  addition  to  the  risk  of  doing  more  harm  than  good  through  a  poor  choice  of  W  in  the  data  fusion 
method  above,  there  are  other  things  that  cause  data  fusion  approaches  to  go  awry.  Hall  and  Garza  have 
enumerated  the  most  common  pitfalls  and  the  following  from  their  list  are  the  most  relevant  to  this  task: 

-  There  is  no  substitute  for  a  good  sensor.  No  amount  of  data  fusion  can  substitute  for  a 
single,  accurate  sensor  that  measures  the  phenomena  that  you  want  to  observe. 

-  Downstream  processing  cannot  make  up  for  errors  (or  failures)  in  upstream 
processing.  Data  fusion  processing  cannot  correct  for  errors  in  processing  (or  lack  of 
preprocessing)  of  individual  sensor  data. 
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-  Sensor  fusion  can  result  in  poor  performance  if  incorrect  information  about  sensor 
performance  is  used.  A  common  failure  in  data  fusion  is  to  characterize  the  sensor 
performance  in  an  ad  hoc  or  convenient  way.  Failure  to  accurately  model  sensor 
performance  will  result  in  corruption  of  the  fused  results.’ 

-  There  is  no  such  thing  as  a  magic  or  golden  data  fusion  algorithm.  Despite  claims  to 
the  contrary,  there  is  no  perfect  algorithm  that  is  optimal  under  all  conditions.  Often,  real 
applications  do  not  meet  the  underlying  assumptions  required  by  data  fusion  algorithms 
(e.g.,  available  prior  probabilities  or  statistically  independent  sources). 

-  Quantifying  the  value  of  a  data  fusion  system  is  difficult .  A  challenge  in  data  fusion 
systems  is  to  quantify  the  utility  of  the  system  at  a  mission  level.  Although  measures  of 
performance  can  be  obtained  for  sensors  or  processing  algorithms,  measures  of  mission 
effectiveness  are  difficult  to  define  [63]. 

For  this  effort,  these  translate  into  an  evaluation  of  how  well  the  algorithm  performs  given  a  particular 
sensor  quality,  a  necessity  to  handle  bad  or  missing  data  as  it  is  presented  to  the  algorithm,  evaluation  of 
the  impact  of  inaccuracies  in  the  assessment  of  the  sensor  quality,  and  the  investigation  of  more  than  one 
fusion/filtering  method  in  order  to  gauge  the  effectiveness  of  the  final  choice. 

Another  form  of  processing  that  is  not  often  associated  with  data  fusion  is  error  minimization.  For  this 
problem,  however,  an  error  function  is  defined,  to  be  detailed  later,  that  incorporates  two  sensor 
measurements.  Error  minimization  is  used  to  combine  these  sensor  measurements  into  a  reduced 
“measurement”  that  minimizes  the  defined  error  function.  In  this  respect,  error  minimization  becomes 
“data  fusion.”  In  cases  such  as  this  where  an  error  function  can  be  devised,  numerous  optimization 
techniques  have  been  developed  to  minimize  the  error  function.  One  method  that  has  found  widespread 
use  in  the  field  of  digital  signal  processing  is  the  “approximate  steepest  descent”  method  known  as  Least 
Mean  Square  (LMS).  Introduced  by  Bernard  Widrow  and  Marcian  Hoff  in  1960,  the  LMS  algorithm 
seeks  to  minimize  the  performance  index  of  mean  square  error  [64],  [65].  Other  commonly  used  methods 
for  minimizing  performance  indices  are  true  “steepest  descent”  methods,  “conjugate  gradient”  methods 
and  variations  of  “Newton’s”  method.  As  pointed  out  by  Hagan  et  al.  [64],  steepest  descent  is  the  simplest 
and  is  almost  certain  to  converge,  but  is  often  slow.  Newton’s  method  is  much  faster,  but  requires  the 
calculation  of  matrix  inverses  and  Hessian  matrices.  Conjugate  gradient  approaches  are  a  compromise 
that  do  not  require  second  derivatives,  but  still  converge  to  the  minimum  of  a  quadratic  performance  index 
in  a  finite  number  of  steps  [64],  Here  I  have  listed  only  a  sample  of  all  the  techniques  available,  and  it 
should  be  clear  that  there  are  many  suitable  approaches  for  finding  the  minimum  of  a  quadratic  error 
surface  such  as  mean-square-error  (MSE).  It  will  be  shown  that  the  performance  surface  of  interest  here  is 
indeed  a  “squared-error”  surface.  The  application  investigated  in  this  thesis,  however,  requires  not  only 
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accurate  minimization  of  this  quadratic  error  surface,  but  also  speed.  The  issue  of  speed  is  driven  by  the 
desire  to  develop  an  algorithm  sufficiently  fast  to  be  run  in  real  time,  with  minimum  computer  resources. 


This  requirement  for  a  fast  routine  narrows  the  field  of  suitable  methods  somewhat.  While  there  are  still 
numerous  choices  that  could  be  argued  to  be  superior,  the  variations  on  the  Newton  method  have  the 
desired  characteristics  of  being  relatively  fast,  having  the  quadratic  convergence  property,  and  are  not 
overly  complex.  As  derived  in  detail  by  Hagan  et  al.  [64],  the  Newton  method  locates  a  stationary  point 
for  quadratic  functions  in  one  iteration,  but  has  the  disadvantage  of  requiring  the  computation  and  storage 
of  the  Hessian  matrix.  Since  this  matrix  is  composed  of  second-order  derivatives,  its  calculation  can  be  a 
difficult  and  time-consuming  process  if  the  functions  are  complicated  or  not  well  known.  It  is  also  pointed 
out  that  the  convergence  properties  of  Newton’s  method  can  be  “quite  complex.”  The  Gauss-Newton 
method  is  a  modification  that  overcomes  some  of  the  disadvantages  of  the  standard  method.  The  main 
advantage  of  Gauss-Newton  over  the  standard  Newton’s  method  is  that  it  still  yields  rapid  convergence, 
but  does  not  require  calculation  of  second  derivatives.  This  is  due  to  the  fact  that  only  the  Jacobian  must 
be  calculated,  not  the  Hessian.  Based  on  its  characteristics,  I  have  concluded,  as  did  the  authors  in  [12], 
that  Gauss-Newton  is  best  suited  to  the  application  in  this  thesis. 


The  Gauss-Newton  method,  as  developed  by  [64],  begins  with  the  standard  Newton  method.  While  its 
exact  form  is  detailed  in  a  later  section,  the  performance  index,  F(q)>  is  a  sum  of  squares  that  is  a  function 
of  the  attitude  quaternion,  q.  The  quaternion  that  optimizes  the  performance  index  is  found  iteratively 
using 
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qk+l  =  qk  ~  Ak  8k 


(7.6) 


where  =  V  F(q) 


<Fqk  =  Vf(?) 


.  As  is  the  case  for  this  application,  when  the 

q=qk 


performance  index  is  a  sum  of  squares,  F(q)  may  be  represented  as  a  function  of  the  error  vector,  £,  as 

N  2  T 

F(q)  =  h£i iq)  =  e  (q)£(q)’  (7-7) 


and  the  jth  element  of  the  gradient  is 


dF(q)  j  de. ( q ) 

[VF(q)]  = -  =  2lf.  ( q ) - .  (7.8) 

dq  .  dq  . 
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This  means  that  the  gradient  can  be  written  in  matrix  form  as 
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VF(q)  =  2J  (q)e(q) 


(7.9) 
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is  the  Jacobian  matrix.  For  the  standard  Newton’s  method,  you  next  must  calculate  the  Hessian  matrix: 
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This  can  be  written  in  matrix  form  as 


where 


2  T 

V  F(q)  =  2 J  (, q)J(q)  +  2S(q ), 
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S(q )  =  X*.  (*)V  £  A q). 
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If  S(c/)  is  assumed  small,  the  Hessian  is  approximated  as 

2  T 

V  F(q)  =  2J  (*)/(*) 

substituting  equations  (7.14)  and  (7.9)  into  equation  (7.6)  yields  the  Gauss-Newton  method: 

=  qk  -  VT  (qk  )J(qk  )f 1  JT  (qk  )e(qk  )  . 


(7.12) 


(7.13) 


(7.14) 


(7.15) 


Here  it  is  clear  that  Gauss-Newton  only  requires  first-order  derivatives  in  the  Jacobian,  as  opposed  to  the 
second-order  derivatives  necessary  to  calculate  the  Hessian  for  the  standard  Newton  method.  Two 
disadvantages  of  this  algorithm  are  that  the  matrix  H-JrJ  may  not  be  invertible,  and  that  storage  space 
must  be  allocated  for  this  matrix.  If  inversion  of  the  H  matrix  becomes  problematic,  a  modified  Gauss- 
Newton  method  called  the  Levenberg-Marquardt  algorithm  may  be  used  to  guarantee  invertibility  [64]. 
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7*3  Wahba’s  Problem  and  Gauss-Newton  Iteration 

The  approach  in  this  thesis  for  determining  the  attitude  quaternion  follows  the  problem  first  published  in 
1965  by  Grace  Wahba  [66].  In  this  problem,  “Wahba  proposed  an  attitude  solution  by  matching  two  non¬ 
zero,  non-collinear  vectors  that  are  known  in  one  coordinate  frame,  and  measured  in  another  [14].”  The 
details  of  the  development  follow  in  a  later  chapter,  but  it  will  be  seen  that  an  error  function  is  devised  that 
is  based  on  the  differences  between  known  and  measured  vectors.  This  error  function  is  of  the  form  [11] 

F(q)  =  eT £  =  (y'  -  U(q)yB  )*  (y*  -  M(q)yB  )  (7.16) 

where 

F  (q )  =  error  function  to  be  minimized, 

€  =  error  vector  (6x  1 ), 

_/ 

y  =  reference  vector  in  the  “inertial”  frame  (6x1), 

M(<?)  =  transformation  matrix  from  “body”  to  “inertial”  frame  (6x6), 

_B 

y  =  measurement  vector  in  the  “body”  frame  (6x1). 

The  reference  vector  in  this  case  is  a  concatenation  of  the  Sun  vector  in  the  inertial  frame  and  the  magnetic 
field  vector  in  the  inertial  frame.  These  vectors  come  from  models  or  look-up  tables  at  each  time  step. 
The  measurement  vector  is  a  concatenation  of  the  Sun  and  magnetic  field  vectors  measured  in  the  body 
frame  by  the  sensors.  The  “front  end”  of  the  algorithm  will  seek  to  minimize  this  error  function. 
Examination  of  F(q)  reveals  that  it  is  a  quadratic  function,  and  certain  methods  are  geared  toward,  or 
better  suited  for,  the  manipulation  of  quadratics.  This  being  the  case,  a  driving  factor  in  selecting  a  data 
fusion  approach  is  to  choose  one  that  efficiently  and  successfully  accomplishes  this  minimization. 

As  discussed  in  the  previous  section,  the  methods  best  suited  to  minimizing  a  quadratic  function,  as 
presented  here,  are  the  LMS  method  and  the  variations  on  Newton’s  method.  Based  on  the  need  for  rapid 
convergence  and  a  minimized  computation,  in  order  to  facilitate  real-time  implementation,  the  Gauss- 
Newton  approach  stands  out  at  the  most  promising  method  and  will  be  implemented  in  this  algorithm.  A 
detailed  development  of  this  implementation  and  an  in-depth  investigation  of  its  convergence  properties 
for  this  application  will  be  covered  in  later  sections  of  this  work. 

Having  selected  a  promising  sensor  fusion  method  from  the  many  available,  it  is  now  necessary  to 
evaluate  “filtering”  methods  that  use  information  from  data  taken  over  time  in  order  to  provide  a  best 


40 


estimate  at  a  given  time  of  interest.  The  next  chapter  steps  through  the  evaluation  of  a  number  of  those 
methods  to  determine  the  best  approach  for  this  effort. 
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8.0  Applicable  Filtering  Techniques 
8*1  Introduction 

As  outlined  in  the  last  chapter,  a  distinction  is  made  between  data  fusion  and  filtering  in  this  work. 
Filtering  describes  the  combination  of  measurements  gathered  over  time  to  generate  the  best  estimate  of  a 
particular  quantity  of  interest.  These  measurements  may  come  from  a  single  sensor  or  from  multiple 
sensors.  The  last  chapter  investigated  those  data  fusion  methods  that  lend  themselves  to  the  solution  of  the 
problem  under  study.  This  chapter  takes  a  similar  survey  of  filtering  techniques  that  are  most  applicable  to 
the  low-cost  rocket  attitude  determination  problem. 

8.2  Promising  Filtering  Techniques 

There  are  many  filtering  approaches,  and  variations  on  well-known  techniques,  that  have  been  devised  to 
handle  the  state  estimation  problem  [67].  The  goal  of  this  research  effort,  with  respect  to  filtering,  is  to 
evaluate  the  most  promising  techniques  and  to  identify  and  implement  one  or  more  that  provide  the  most 
leverage  with  respect  to  yielding  the  best  attitude  estimate  given  the  constraints  of  the  low-cost  approach. 
In  addition  to  being  constrained  to  relatively  noisy  sensors  and  a  limited  number  of  them,  there  are  the 
challenges  of  the  problem  itself.  Sounding  rockets  are  highly  dynamic  vehicles  and  their  attitude  state  can 
change  rapidly  and  over  a  broad  range  of  values.  Additionally,  the  equations  governing  the  solution  for 
attitude  are  nonlinear  by  nature.  These  problem  parameters  limit  the  number  of  filtering  approaches  that 
will  enjoy  success  for  this  application.  While  many  approaches  were  reviewed,  this  section  discusses 
7those  that  held  out  the  most  promise  and  evaluates  their  suitability  for  this  problem. 

8.2.1  Extended  Kalman  Filter 

Despite  the  recent  appearance  of  alternatives  for  the  filtering  of  nonlinear  systems,  the  extended  Kalman 
filter  (EKF)  remains  the  most  popular  approach  due  to  its  simplicity,  familiarity,  and  successful 
application  [68],  [69],  [70].  Entire  works  have  been  devoted  to  the  roots  of,  modifications  to,  and 
characteristics  of  the  Kalman  filter.  Three  excellent  references  are  [17],  [71]  and  [72],  and  there  are  many 
more.  Here  I  will  focus  on  a  brief  history  and  a  summarization  of  the  characteristics  of  the  Kalman  filter 
as  they  apply  to  this  problem. 

The  original  formulation  of  the  Kalman  filter  is  attributed  to  R.  E.  Kalman  and  was  first  published  in  1960 
[73].  In  its  original  form,  it  was  designed  as  a  statistical  approach  to  estimate  the  state  of  a  system 
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described  by  linear  differential  equations,  expressed  in  state-space  form,  where  the  measurements  are 
linear  functions  of  the  state  [17],  [74].  Such  a  system  may  be  expressed  as: 


x  -  Fx  +  w 
z  =  Hx  +  v 


(8.1) 


where  x  is  the  system  state  vector,  F  is  the  system  fundamental  or  dynamics  matrix,  vv  is  a  process  noise 
vector,  H  is  the  measurement  matrix,  z  is  the  measurement  vector,  and  v  is  a  vector  representing  the 
measurement  noise  components. 


One  of  the  great  advantages,  for  the  linear  system  case,  is  that  the  prediction  step  makes  use  of  the  state 
transition  matrix  to  propagate  the  system  states  forward  using  only  matrix  multiplication  [17].  In 
estimating  the  state  vector  at  the  next  time  step,  a  Kalman  filter  “utilizes  a  weighting  function,  called  the 
Kalman  gain,  which  is  optimized  to  produce  a  minimum  error  variance.  For  this  reason,  the  Kalman  filter 
is  called  an  optimal  filter.  For  linear  system  models,  the  Kalman  filter  is  structured  to  produce  an  unbiased 
estimate  [74].” 


While  this  is  a  very  powerful  filtering  method,  its  traditional  form  is  limited  to  linear  problems,  or  those 
that  are  only  slightly  nonlinear.  Since  many  real-world  problems,  including  attitude  determination  of  a 
sounding  rocket,  are  characterized  by  either  nonlinear  system  equations,  or  nonlinear  measurement 
equations,  a  method  suitable  to  handling  these  problems  was  needed. 

Very  early  on,  the  standard  Kalman  filter  structure  was  modified  to  handle  nonlinear  problems.  Two 
approaches  are  commonly  taken.  The  first  is  known  as  a  linearized  Kalman  filter  in  which  the  system 
model  is  linearized  about  a  nominal  trajectory  before  filtering  begins.  This  approach  is  often  robust  with 
respect  to  poor  initialization,  but  does  not  respond  well  if  the  actual  trajectory  being  estimated  does  not 
closely  match  that  used  to  linearize  the  system  up  front  [17].  Since  it  is  feasible  that  a  rocket’s  attitude 
will  deviate  from  the  expected  motion,  the  linearized  Kalman  filter  is  not  well-suited  to  this  problem.  A 
second,  more  common,  approach  is  known  as  the  extended  Kalman  filter.  This  development  is  attributed 
to  Schmidt  and  is  sometimes  called  the  Kalman-Schmidt  filter  [71].  In  this  case,  the  trajectory  is 
linearized  about  the  estimated  trajectory,  at  each  time  step,  and  evaluated  using  the  current  estimate  of  the 
state  vector.  While  this  approach  is  more  susceptible  to  initialization  problems,  it  is  more  robust  in 
situations  where  the  actual  trajectory  varies  significantly  from  the  expected  trajectory  [17],  [71],  This  is 
the  approach  that  will  be  evaluated  in  this  work. 

The  extended  Kalman  filter  operates  on  a  representation  for  a  nonlinear  system  that  is  similar  to  the 
representation  for  a  linear  system  found  in  (8.1) 


43 


x  =  f(x)  +  w 
z  =  A(3c)  +  v 


(8.2) 


where  f(x)  and  h(x)  may  now  be  nonlinear  functions  of  the  state  vector.  The  extended  Kalman  filter  is 

a  recursive  method  of  solving  the  nonlinear  system  indicated  above.  It  is  recursive  in  the  sense  that 
Each  updated  estimate  of  the  state  is  computed  from  the  previous  estimate  and  the  new 
input  data,  so  only  the  previous  estimate  requires  storage.  In  addition  to  eliminating  the 
need  for  storing  the  entire  past  observed  data,  the  Kalman  filter  is  computationally  more 
efficient  than  computing  the  estimate  directly  from  the  entire  past  observed  data  at  each 
time  step  of  the  filtering  process  [67]. 


While  a  more  detailed  development  will  follow  in  Chapter  9,  a  brief  overview  of  the  filter  structure  is 
given  here.  Following  the  development  in  [17],  the  discrete  extended  Kalman  filter  can  be  considered  to 
be  based  on  the  following  recursive  equations.  The  new  estimate  of  the  state  vector,  x ,  is  formed  at  each 
time  step  by  updating  the  projected  value  of  the  state  vector,  x ,  with  a  correction  based  on  the  projected 

* 

observations,  z  ,  relative  to  the  actual  observations,  z  ,  according  to 

x  =  x  +  K(z  -z)  (8.3) 

where  K  is  known  as  the  Kalman  gain.  The  recursive  equations  serve  the  purpose  of  generating  each  of 
the  terms  in  (8.3)  in  order  to  calculate  the  new  estimate,  Jc .  There  are  three  main  equations,  known  as  the 
Riccati  equations  that  are  calculated  during  each  iteration,  or  time  step.  The  first  calculates  what  is  known 
as  the  covariance  of  the  error  in  the  estimate  of  the  state  vector,  M.  This  is  further  qualified  to  define  M 
as  the  covariance  of  the  error  in  the  estimate  before  the  update,  meaning  during  the  current  time  step,  but 
before  the  new  observation  has  been  processed  by  the  filter.  Essentially,  M  is  the  filter’s  own  assessment 
of  how  good  its  estimate  is.  It  is  calculated  as 

Mk^khWk^Qk  (8-4) 

where  <I>*  is  the  state  transition  matrix  at  time  step  k ,  P*./  is  the  covariance  of  the  error  in  the  estimate  at 
the  previous  time  step  after  the  update,  or  when  the  observation  had  already  been  processed  by  the  filter, 
and  Qk  is  the  discrete  process  noise  matrix,  which  is  yet  another  covariance  matrix  that  represents  the 
filter’s  assessment  of  how  good  the  embedded  dynamic  model,  or  process  is.  As  noted  earlier,  in  a  linear 
system,  the  state  transition  matrix  may  be  used  to  calculate  the  state  vector  at  any  other  point  in  time, 
through  matrix  multiplication.  Unfortunately,  for  the  EKF,  this  is  no  longer  possible,  because  the  state 
transition  matrix  is  based  on  a  truncated  Taylor  series  expansion  that  results  in  a  first-order  linearization  of 
the  actual  nonlinear  system  dynamics  equations  represented  by  f(x) .  From  linear  systems  theory,  the 
continuous  time  state  transition  matrix  may  be  represented  as 
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Ft 

0(0  =  e  (8.5) 

where  F  is  the  linearized  dynamics  matrix  for  the  system  found  using 

d/(x) 

F  = -  .  (8.6) 

X=x 

This  equation  is  evaluated  at  the  state  estimate  at  each  time  step  to  get  a  new  linearization  of  the  dynamics 
matrix.  Equation  (8.5)  represents  the  continuous  time  version  of  the  state  transition  matrix,  and  the  matrix 
exponential  may  be  represented  as  an  infinite  Taylor  series  expansion  of  the  form 

Ft 

O(f)  =  e  ~  I  +  Ft  (8.7) 

where  only  the  first  two  terms  of  the  expansion  are  retained.  This  amounts  to  an  additional  linearization 
that  assumes  that  the  filter  time  step  is  sufficiently  small  such  that  the  linearization  is  valid.  Through 
extensive  research,  Zarchan  has  demonstrated  that  for  most  cases,  no  significant  performance  gain  is 
achieved  by  retaining  additional  terms  [17].  This  may  be  traced  back  to  the  fact  that  the  state  transition 
matrix  is  only  used  as  a  means  to  calculate  the  Kalman  gain,  and  not  to  propagate  the  states  forward  as 
would  be  the  case  in  a  linear  system.  For  the  EKF,  the  states  are  propagated  through  the  numerical 
integration  of  the  actual  nonlinear  dynamics  equations  embodied  in  f(x )  because  this  is  more  accurate 
than  propagating  through  matrix  multiplication  using  a  linearized  state  transition  matrix.  The  discrete 
process  noise  matrix,  Q&  is  a  function  of  the  continuous  time  state  transition  matrix,  <I>(r),  and  the 
continuous  time  process  noise  matrix,  Q(t),  which  is  yet  another  covariance  matrix  defined  as 

T 

Q(t)  =  E[ww  ].  (8.8) 

It  is  intended  to  represent  the  uncertainty  in  the  embedded  process  model,  or  how  good  the  dynamics 
equations  are.  The  initialization  of  this  and  the  other  matrices  in  this  section  is  discussed  in  Chapter  9. 
The  discrete  form  of  the  process  noise  matrix  is  recalculated  at  each  iteration  to  update  the  filter’s 
assessment  of  how  well  the  embedded  model  is  performing.  This  done  using  the  equation 

T  T 

Q  =Iqs^(t)Q(t^  ( T)dt .  (8.9) 

k 

The  remaining  elements  needed  to  calculate  the  covariance  before  the  update,  Af,  is  the  covariance  after 
the  update  from  the  last  time  step,  P ,  and  the  discrete  form  of  the  state  transition  matrix,  O*.  Os  is  found 
by  merely  substituting  Ts ,  the  filter  time  step,  in  for  continuous  t  in  (8.7).  P  is  the  filter’s  assessment  of 
how  good  its  estimate  of  the  state  vector  is  after  it  has  processed  the  new  observation.  This  covariance 
matrix,  which  may  be  interpreted  as  a  diagonal  matrix  with  the  squared  error  of  each  state  estimate  along 
its  diagonal,  is  calculated  at  each  iteration  using 
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Pk  =  (/  -  KkH^Mk  (81°) 

where  /  is  the  identity  matrix  and  H  is  the  linearized  measurement  sensitivity  matrix  found  by  linearizing 
h(x)  from  (8.2)  by  taking  partials  similar  to  what  was  done  to  get  F  in  (8.6).  This  is  the  second  of  the 
Riccati  equations  that  are  iterated  in  a  traditional  Kalman  filter.  The  last  of  the  three  is  the  equation  that 
calculates  the  Kalman  gain  at  each  time  step.  It  is  found  using 

K  =  MHT  [HMHT  +  R]~l  (8.11) 

where  all  but  R  have  already  been  defined.  R  is  the  last  of  the  covariance  matrices  and  represents  the 
uncertainty,  or  error,  in  the  measurements.  It  is  defined  as 

T 

R  =  E[vv  ]  (8.12) 

where  v  is  the  vector  from  (8.2)  that  represents  the  statistical  noise  process  that  corrupts  the  measurements. 
Given  the  expectation  operator  in  (8.12),  and  assuming  a  zero  mean  sensor  error,  the  diagonal  elements  of 
this  matrix  may  be  interpreted  as  the  variances  of  the  sensor  measurements.  Or  similarly,  the  square  root 
of  the  diagonal  elements  may  be  interpreted  as  the  standard  deviations  of  the  sensor  measurements.  In 
fact,  this  is  how  this  particular  matrix  is  initialized  as  will  be  detailed  in  the  next  chapter. 

Looking  back  to  (8.3),  we  see  that  the  whole  point  of  this  endeavor  is  to  generate  a  new  estimate  of  the 
state  vector,  x.  jc  is  the  projected  state  vector  at  the  next  time  step  and  is  found  by  numerically 
integrating  the  nonlinear  state  equations  forward  one  time  step,  z  is  the  projected  observation  and  is 
found,  in  general,  by  numerically  integrating  the  nonlinear  measurement  equations  from  (8.2)  forward  one 

* . 

time  step,  and  evaluating  the  result  at  the  projected  values  of  the  states.  Finally,  z  is  the  actual 
observation,  usually  coming  from  the  sensors.  With  all  of  these  pieces  in  place,  a  new  state  estimate  is 
formed,  and  the  process  begins  again. 

The  Kalman  filter  is  often  proclaimed  to  be  an  “optimal”  filtering  method  for  state  estimation  and  in  fact  it 
is  the  “minimum  mean-square  (variance)  estimator  of  the  state  of  a  linear  dynamical  system  [67].”  As 
Gelb  points  out,  however,  “The  truly  optimal  filter  must  model  all  error  sources  in  the  system  at 
hand... Also,  it  is  assumed  in  the  filter  equations  that  exact  descriptions  of  system  dynamics,  error  statistics 
and  the  measurement  process  are  known  [75].”  These  two  assumptions  must  certainly  be  violated  in  every 

real  world  problem.  A  key  example  is  the  treatment  of  both  the  process  noise,  wr ,  and  the  measurement 
noise,  vr .  These  are  modeled  as  white,  Gaussian  and  zero-mean  and  two  fundamental  assumptions  of  the 
Kalman  filter  are  that  the  unmodeled  measurement  errors  are  white  (i.e.,  uncorrelated),  and  the  unmodeled 
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error  dynamics  are  white  (i.e.,  uncorrelated)  [76].  While  these  may  be  good  assumptions  from  the 
perspective  that  using  them  produces  acceptable  results,  they  are  most  likely  not  true  in  a  strict  sense. 

It  is  clear  from  this  development  that  the  EKF  is  not  really  a  “nonlinear”  filtering  method,  but  really  a 
modification  of  a  linear  approach  to  handle  a  nonlinear  problem.  Unfortunately,  despite  being  the  most 
common,  and  perhaps  best  known,  filtering  technique  for  nonlinear  problems,  the  EKF  has  a  number  of 
well  established  shortcomings  [69]  First,  the  linearization  required  to  apply  the  Kalman  filter  structure 
developed  for  linear  systems  to  nonlinear  systems  can  result  in  highly  unstable  filters.  As  Julier  and 
Uhlmann  point  out,  this  is  especially  true  if  the  assumption  of  local  linearity  is  violated.  Risk  of  this  is 
minimized  if  very  short  time  steps,  relative  to  the  time  scales  associated  with  changes  in  the  states,  are 
used.  Second,  “linearization  can  be  applied  only  if  the  Jacobian  matrix  exists,  and  the  Jacobian  matrix 
exists  only  if  the  system  is  differentiable  at  the  estimate  [69].”  Additionally,  calculation  of  the  Jacobian,  if 
it  does  exist,  is  often  not  a  trivial  undertaking.  At  the  very  least,  it  requires  a  significant  amount  of  algebra 
as  evidenced  by  the  derivation  for  this  application,  to  be  seen  in  Chapter  9.  Haykin  highlights  another 
well-known  liability  of  the  EKF,  that  it  is  prone  to  stability  problems  due  to  numerical  difficulties,  with 
the  following: 

For  example,  the  posteriori  covariance  matrix  Pk  is  defined  as  the  difference  between  two 
matrices... Hence,  unless  the  numerical  accuracy  of  the  algorithm  is  high  enough,  the 
matrix  Pk  resulting  from  this  computation  may  not  be  nonnegative-definite.  Such  a 
situation  is  clearly  unacceptable,  because  Pk  represents  a  covariance  matrix.  The  unstable 
behavior  of  the  Kalman  filter,  which  results  from  numerical  inaccuracies  due  to  the  use  of 
finite- word  length  arithmetic,  is  called  the  divergence  phenomenon  [67]. 

Numerical  issues  are  also  noted  in  [17],  [71],  and  [77].  Additional  implementation  concerns,  timing 
issues,  and  tuning  problems  appear  in  the  literature  as  well  [69].  All  of  these  issues  can  contribute  to 
decreased  accuracy  of  the  state  estimate  produced  by  the  EKF.  Despite  its  successful  application  in  many 
nonlinear  applications,  the  drawbacks  noted  here  give  impetus  to  the  search  for  nonlinear  filtering  methods 
that  are  not  a  “linear  method  made  to  work.” 

8.2.2  Variations  on  the  Extended  Kalman  Filter:  Adaptive  Kalman  Filter  /  Multiple-Model  Filter 

A  variation  on  the  EKF  is  the  Adaptive  Kalman  Filter.  While  the  overall  structure  is  the  same  as  that 
described  in  the  preceding  chapter,  a  Kalman  filter  may  be  made  “adaptive”  by  including  some  of  the 
dynamic  equation  parameters,  or  noise  parameters,  in  the  state  vector  [78],  [79],  [80].  By  increasing  the 
size  of  the  state  vector  in  this  fashion,  one  or  more  of  the  parameters  embedded  in  the  state  dynamic 
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equations  are  allowed  to  change  and  their  value  is  estimated  at  each  filtering  time  step.  Then  the  new 
parameter  values  are  used  for  the  next  iteration. 

This  approach  is  probably  most  useful  in  situations  where  the  dynamic  model  is  expected  to  change 
significantly  over  time.  For  example,  if  a  vehicle’s  structure  changes  by  losing  a  nose-cone,  one  would 
expect  that  the  coefficient  of  drag  for  the  vehicle  will  change  significantly.  If  the  vehicle’s  position  and 
velocity  states  are  being  estimated,  the  filter  will  likely  rely  on  a  kinematic  equation  of  motion  to  predict 
the  position  and  velocity  at  the  next  time  step  [17].  This  dynamic  model  almost  certainly  will  incorporate 
the  coefficient  of  drag.  Including  the  coefficient  of  drag  as  an  estimated  state  might  help  the  filter  better 
estimate  the  position  and  velocity  states.  Of  course,  increasing  the  size  of  the  state  vector  also  increases 
the  dimension  of  many  of  the  matrices  used  for  a  Kalman  style  filter,  thereby  increasing  the  computational 
load.  This  approach  is  best  suited  to  situations  where  the  model  is  well  understood,  as  in  the  example 
above,  but  where  parameter  values  may  change.  As  discussed  in  Chapter  5,  the  dynamic  model  for  the 
rocket’s  rotational  motion  is  complex,  and  the  desire  is  to  make  this  algorithm  both  time  efficient  for  real 
time  implementation  and  easily  transportable  between  different  vehicles.  As  a  result,  a  generic,  simplified 
dynamic  model  is  employed,  and  the  approach  described  here  is  not  well-suited  to  this  effort. 

Another  variation  on  an  “adaptive”implementation  of  a  Kalman  style  filter  is  the  Multiple  Model  Adaptive 
Estimator  (MMAE)  approach  described  by  Hanlon  and  Maybeck  [81].  Similar  to  the  adaptive  Kalman 
filter  described  above,  this  approach  handles  the  situation  where  the  general  model  is  known,  but  the  exact 
model  is  not  known.  Instead  of  attempting  to  estimate  the  unknown  parameters  in  the  model,  multiple 
Kalman  style  filters  are  implemented  in  parallel,  each  with  a  different  model.  Then  a  “hypothesis”  test  is 
accomplished  on  the  results  from  each  filter  in  order  to  determine  which  provides  the  best  results.  The 
drawback  for  this  application  is  the  additional  computing  power  needed  to  implement  multiple  filters  in 
parallel.  It  also  requires  higher  fidelity  models  than  the  model  to  be  implemented  here.  This  approach  is 
applied  in  [81]  to  flight  control  sensor/actuator  failure  detection  and  identification  and  a  similar  approach 
is  described  by  Rama  for  application  to  target  tracking  [82].  Multiple  model  filtering  seems  best  suited  to 
such  applications  where  several  realistic  models  can  be  produced  to  cover  different  situations. 
Additionally,  each  individual  filter  suffers  from  the  same  drawbacks  described  for  the  individual  filter  in 
the  previous  section.  For  these  reasons,  this  approach  is  not  pursued  for  this  work. 

8.2.3  Alternatives  to  the  Extended  Kalman  Filter 

A  number  of  other  filtering  approaches  were  investigated  for  this  nonlinear  problem.  Having  established 
that  the  EKF  is  the  traditional  approach,  the  goal  was  to  identify  any  other  promising  technique  that  could 
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be  applied  here  to  better  estimate  rocket  attitude  given  the  problem  constraints. 

One  alternative  to  the  EKF  is  the  complementary  filter.  This  approach  has  been  used  successfully  in  lieu 
of  an  EKF  by  several  authors.  One  example  is  described  in  [15]  where  a  complementary  filter 
successfully  combines  data  from  accelerometers,  magnetometers  and  rate  sensors  to  generate  an  attitude 
estimate.  The  rate  sensor  data  is  used  to  determine  orientation  of  the  object,  but  due  to  sensor  accuracy, 
this  is  only  possible  for  short  periods.  This  problem  is  overcome  by  using  “complementary”  data  from 
other  sensors  to  “recalibrate”  the  solution  and  combining  both  sets  of  data  using  the  complementary  filter 
structure  [15].  This  type  of  filter  is  typically  used  when  statistical  properties  of  the  measurements  and 
dynamic  processes  are  not  known  to  sufficient  accuracy  [13].  McGhee  et  al.  [13],  also  point  out  that  the 
most  serious  drawback  to  “optimal”  filters  such  as  the  EKF  is  that  they  are  highly  “tuned”  to  the  assumed 
problem  statistics.  The  complementary  filter  structure  simplifies  the  “tuning”  process  by  employing  a 
single  parameter  which  is  adjusted  for  best  performance,  as  opposed  to  several  parameters  in  the  case  of 
the  EKF.  For  this  application,  the  statistics  are  expected  to  be  fairly  well  defined,  and  it  is  desirable  to 
estimate  not  only  orientation,  but  also  attitude  rates.  As  opposed  to  correcting  the  solution  from  one  set  of 
sensors  periodically,  a  solution  as  close  to  “optimal”  is  desired.  Given  this  scenario,  and  lacking  any 
noteworthy  performance  increase  over  the  EKF,  the  traditional  EKF  appears  to  be  better  suited  to  this 
problem. 

Another  area  that  was  reviewed  which  has  received  a  great  deal  of  recent  interest,  and  shows  much 
promise  for  certain  applications,  is  “fuzzy  logic.”  The  proclaimed  advantages  over  traditional  methods 
include  applications  where  the  system  is  difficult  to  model,  or  where  ambiguity  or  vagueness  is  common. 
Fuzzy  logic  can  be  described  as  a  structured,  model-free  estimator  that  approximates  a  function  through 
linguistic  input/output  associations.  In  this  sense,  fuzzy  systems  are  rules-based  as  opposed  to  more 
traditional  methods  that  use  strict  decisions  and  assignments  [83].  For  example,  a  room  might  be 
described  as  “hot”  instead  of  as  having  a  temperature  >  100  degrees.  Decisions  for  control  or  estimation 
would  then  be  based  on  this  linguistic  interpretation  of  the  situation.  Given  the  desire  to  devise  a 
transportable  algorithm  to  be  used  for  a  wide  variety  of  rockets,  this  model-free  approach  seemed 
promising,  at  first.  Upon  further  investigation,  however,  it  is  unclear  how  such  a  linguistic  interpretation 
might  be  applied  to  this  problem  and  other,  more  traditional,  approaches  appear  to  be  more  promising. 

Finally,  relatively  recent,  and  potentially  revolutionary,  developments  in  nonlinear  filtering  are  the  so- 
called  “derivative-free”  filtering  methods.  “Derivative-free”  refers  to  the  fact  that  no  explicit  Jacobian  or 
Hessian  matrices  must  be  calculated  as  is  the  case  when  linearizing  to  implement  an  EKF,  for  instance. 
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The  most  notable  among  these  is  the  Unscented  Kalman  Filter  (UKF).  Julier  and  Uhlmann  describe  the 
unscented  transform  (UT),  upon  which  the  UKF  is  based,  as 

a  mechanism  for  propagating  mean  and  covariance  information  through  nonlinear 
transformations... (that)  is  more  accurate,  is  easier  to  implement,  and  uses  the  same  order 
of  calculations  as  the  EKF.  Furthermore,  the  UT  permits  the  use  of  Kalman-type  filters 
in  applications  where,  traditionally,  their  use  was  not  possible  [69], 

The  UT  is  a  new  method  for  handling  the  nonlinear  transformation,  superior  to  linearization  in  many 
respects,  that  is  “founded  on  the  intuition  that  it  is  easier  to  approximate  a  probability  distribution  than  it 
is  to  approximate  an  arbitrary  nonlinear  function  or  transformation.[69Y  In  their  analysis  of  this  recently 
developed  transformation,  Julier  and  Uhlmann  go  on  to  detail  a  number  of  the  characteristics  that  make  the 
UKF  superior  to  the  EKF  for  problems  that  embody  a  nonlinear  transformation: 

-  The  UT  is  demonstrably  superior  to  linearization  in  terms  of  expected  error  for  all 
absolutely  continuous  nonlinear  transformations.  The  UT  can  be  applied  with 
nondifferentiable  functions  in  which  linearization  is  not  possible. 

-  The  UT  avoids  the  derivation  of  Jacobian  (and  Hessian)  matrices  for  linearizing 
nonlinear  kinematic  and  observation  models.  This  makes  it  conducive  to  the  creation  of 
efficient,  general-purpose  “black  box”  code  libraries. 

-  Empirical  results  for  several  nonlinear  transformations  that  typify  those  arising  in 
practical  applications  clearly  demonstrate  that  linearization  yields  very  poor 
approximations  compared  to  those  of  the  UT  [69]. 

While  Julier  and  Uhlmann  are  credited  with  the  original  development  of  the  UKF,  Wan  and  van  der 
Merwe  are  recognized  as  having  further  developed  the  concept  [68].  Wan  and  van  der  Merwe  give  an 
excellent  synopsis  of  the  differences  between  the  UKF  and  the  EKF: 

The  basic  difference  between  the  EKF  and  UKF  stems  from  the  manner  in  which 
Gaussian  random  variables  (GRV)  are  represented  for  propagating  through  system 
dynamics.  In  the  EKF,  the  state  distribution  is  approximated  by  a  GRV,  which  is  then 
propagated  analytically  through  the  first-order  linearization  of  the  nonlinear  system.  This 
can  introduce  large  errors  in  the  true  posterior  mean  and  covariance  of  the  transformed 
GRV,  which  may  lead  to  suboptimai  performance  and  sometimes  divergence  of  the  filter. 

The  UKF  addresses  this  problem  by  using  a  deterministic  sampling  approach.  The  state 
distribution  is  again  approximated  by  a  GRV,  but  is  now  represented  using  a  minimal  set 
of  carefully  chosen  sample  points.  These  sample  points  completely  capture  the  true  mean 
and  covariance  of  the  GRV,  and,  when  propagated  through  the  true  nonlinear  system, 
capture  the  posterior  mean  and  covariance  accurately  to  second  order  (Taylor  series 
expansion)  for  any  nonlinearity.  The  EKF,  in  contrast,  only  achieves  first-order  accuracy. 
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No  explicit  Jacobian  or  Hessian  calculations  are  necessary  for  the  UKF.  Remarkably,  the 
computational  complexity  of  the  UKF  is  the  same  order  as  that  of  the  EKF  [68]. 

Essentially,  this  new  approach  to  propagating  a  mean  and  covariance  through  a  nonlinear  transformation 
hinges  on  choosing  a  set  of  sample  points  that  accurately  captures  the  information,  propagating  the  points 
through  the  actual  nonlinear  transformation,  and  finally  calculating  weighted  summations  of  the 
propagated  points  to  estimate  the  new  mean  and  covariance  at  the  next  time  step.  This  process  is 
illustrated  for  a  two  dimensional  case  by  Figure  8.1  below,  taken  from  [68].  This  figure  compares  the 


y  =  f(x) 
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weighted  sample  mean 
and  covariance 


I 


Actual  First-Order  Linearization  (EKF>  Unscented  Transform 


Figure  8.1:  Example  of  the  UT  for  Mean  and  Covariance  Propagation  (Figure  7.3  in  [68]) 


mean  and  covariance  propagation  performance  for  the  EKF  and  the  UT  relative  to  the  actual  which  was 
generated  using  a  Monte  Carlo  approach.  The  UT  may  be  implemented  as  part  of  a  recursive  algorithm, 
using  a  structure  similar  to  that  of  the  EKF,  to  create  what  is  known  as  the  Unscented  Kalman  Filter,  or 
UKF.  In  this  fashion,  the  advantages  of  the  Kalman  style  filter  are  retained,  while  the  method  of 
propagating  the  mean  and  covariance  are  improved. 
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While  the  UKF  appears  to  have  numerous  well-documented  performance  advantages  over  the  EKF  [68], 
[69],  [84],  Wan  and  van  der  Merwe  point  out  a  number  of  limitations.  One  significant  point  is  that  the 
UKF,  like  the  EKF,  still  assumes  a  Gaussian  distribution  for  the  probability  density  function  of  the  state 
variables.  This  limitation  is  mitigated  by  the  fact  that,  as  has  been  demonstrated  with  the  EKF  [17],  this 
assumption  often  proves  to  be  an  adequate  reflection  of  reality  and  “numerous  real-world  applications 
have  been  successfully  implemented  based  on  this  assumption  [68].”  It  is  in  the  treatment  of,  or  more 
accurately  the  propagation  of,  these  random  variables  that  the  UKF  excels  in  comparison  to  the  EKF.  The 
UKF  calculates  the  mean  and  covariance  “to  second  order  or  better  which  means  more  accurate 
implementation  of  the  optimal  recursive  estimation  equations,  which  is  the  basis  for  both  the  EKF  and 
UKF  [68].”  While  the  UKF  equations  appear  to  be  more  complicated,  both  [68]  and  [69]  point  out  that  the 
number  of  calculations  for  both  the  EKF  and  the  UKF  is  on  the  order  of  L3,  where  L  is  the  dimension  of 
the  state  vector,  when  implemented  as  a  state  estimator.  The  apparent  complexity  of  the  equations  is 
offset  by  the  elimination  of  many  of  the  matrix  derivations  and  partial  derivatives  necessary  when 
implementing  the  EKF.  The  UKF  does  incur  a  computation  penalty,  with  respect  to  the  EKF,  in  terms  of 
propagating  all  the  sigma  points  through  the  nonlinear  transformation.  This  is  computationally  intensive 
since  each  is  instantiated  through  the  nonlinear  dynamic  equations  using  numerical  integration.  Finally, 
the  UKF  has  three  parameters  that  must  be  defined  that  govern  how  the  sample  set  of  points  is  chosen  and 
propagated  forward.  The  process  for  determining  these  parameters  is  not  yet  well  delineated,  and  defining 
these  parameters  leads  to  some  additional  uncertainty  in  the  implementation  of  the  UKF  [68].  More 
information  on  this  “tuning”  aspect  of  the  UKF  is  found  in  Chapter  10. 

8.3  Most  Suitable  Filtering  Technique  for  this  Application:  Unscented  Kalman  Filter 

Given  the  discussion  of  Section  8.2,  the  Extended  Kalman  Filter  (EKF)  and  the  Unscented  Kalman  Filter 
(UKF)  were  selected  as  the  most  suitable  approaches  for  the  low-cost  attitude  determination  problem. 
While  the  benefits  described  for  the  UKF  make  it  appear  to  be  the  obvious  choice,  the  goal  of  this  work  is 
to  determine  the  method  that  generates  the  best  answer  given  the  constraints  of  the  low-cost  system.  With 
this  goal  in  mind,  both  EKF-based  algorithms  and  UKF-based  algorithms  are  developed.  In  each  case, 
versions  that  accept  rate  measurements  from  rate  sensors  and  versions  that  rely  on  rate  “measurements” 
from  differencing  quaternion  elements  and  then  converting  to  body  rates  are  designed.  This  provides  an 
opportunity  to  evaluate  the  performance  of  the  UKF-based  algorithm  against  what  has  traditionally  been 
the  standard  approach.  Furthermore,  this  allows  each  algorithm  to  process  the  exact  same  data  under 
controlled  circumstances  for  a  true  evaluation  of  not  only  which  algorithm  performs  the  best,  but  what  the 
absolute  quality  of  the  performance  is.  Chapter  9  describes  the  detailed  design  of  each  of  these 
algorithms,  with  additional  information  on  “tuning”  of  the  filters  found  in  Chapter  10. 
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9.0  Devising  the  Algorithm 

This  chapter  builds  on  the  background  information  laid  out  in  Chapters  3  through  8.  Whereas  those 
chapters  summarized  the  survey  of  current  and  promising  approaches,  this  chapter  details  the  approach  as 
implemented  in  design  of  the  algorithms  in  this  work. 

9.1  Restating  the  Objective 

It  is  important  to  maintain  a  clear  view  of  the  objective  of  this  research.  As  Shuster  and  Oh  point  out: 

A  recurrent  problem  in  spacecraft  attitude  determination  is  to  determine  the  attitude  from  a 
set  of  vector  measurements.  Thus,  an  orthogonal  matrix  A  (the  attitude  matrix  or 
direction-cosine  matrix)  is  sought  which  satisfies 

AVt  =  tff  (9.1) 

where  V^,...,Vn  are  a  set  of  reference  unit  vectors,  which  are  n  known  directions  (e.g.,  the 
direction  of  the  Earth,  the  Sun,  a  star,  or  the  geomagnetic  field)  in  the  reference  coordinate 
system,  and  W j Wn  are  the  observation  unit  vectors  which  are  the  same  n  directions  as 

measured  in  the  spacecraft-body  coordinate  system... Because  both  the  observation  and 
the  reference  unit  vectors  are  corrupted  by  error,  a  solution  for  A  does  not  exist  in  general, 
not  even  for  n  —  2  [35]. 

While  many  approaches  have  been  researched  to  better  solve  this  problem,  the  intent  here  is  to  leverage 
promising  sensor  fusion  and  filtering  techniques  to  devise  an  attitude  determination  algorithm  suitable  for 
implementation  using  low-cost  sensors.  As  pointed  out  in  the  introduction,  many  techniques  have  been 
devised  and  successfully  used  for  applications  where  large  budgets  are  available.  The  low-cost  approach 
is  accompanied  by  a  new  set  of  challenges,  most  stemming  from  the  lower  accuracy  and  lesser  number  of 
sensors  available  when  cost  is  limited.  Here,  the  focus  is  on  what  can  be  done  with  less,  thereby  providing 
a  useful  algorithm  to  the  experimenter  with  limited  resources. 

9.2  Overview  of  the  Approach 

As  supported  in  previous  chapters,  the  traditional  approach  to  this  type  of  estimation  problem  is  based  on 
the  Extended  Kalman  Filter  (EKF).  As  a  baseline  for  this  study,  such  a  filter  is  designed  and  tested.  One 
difference  from  what  might  be  considered  the  “standard”  approach  is  the  use  of  the  Gauss-Newton  error 
minimization  routine  to  reduce  the  complexity  of  the  state  equations  used  to  calculate  the  predicted 
observation  at  the  next  time  step.  A  similar  approach  was  used  by  Marins  for  his  low  dynamic  vehicle 
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using  gravity  (accelerometer),  magnetic  field,  and  rotational  rate  sensors  [11].  Using  this  approach,  four 
derived  measurements  replace  six  actual  measured  quantities.  A  second,  and  perhaps  more  significant 
departure,  is  the  design  and  test  of  an  algorithm  using  the  Unscented  Kalman  Filter  (UKF)  in  place  of  the 
EKF.  This  second  algorithm  still  uses  the  Gauss-Newton  approach  to  simplify  the  state  equations,  but 
takes  advantage  of  the  superior  nonlinear  estimation  characteristics  of  the  UKF  as  compared  to  the  EKF. 
Additionally,  the  feasibility  of  eliminating  the  rate  sensors  in  order  to  further  reduce  system  cost,  is 
investigated.  Both  the  EKF-based  and  the  UKF-based  algorithms  are  tested  with  and  without  rate  sensors 
and  the  comparative  performance  is  analyzed. 

9.2.1  Measurements  Available  to  the  Algorithm 

As  developed  in  Chapter  6,  the  sensors  making  up  the  sensor  suite  for  this  work  consist  of  a  minimum  of  a 
Sun  sensor  and  a  magnetometer.  Depending  upon  the  implementation,  gyroscopes  are  included  to 
measure  rates  in  the  body  frame.  The  detailed  characteristics  of  the  sensors  used  are  discussed  in  a  later 
section.  This  section  concentrates  on  the  type  and  number  of  measurements  to  be  fed  to  the  algorithm.  I 
will  distinguish  between  the  word  “measurement”  meaning  something  coming  out  of  a  sensor,  and  the 
word  “observation”  meaning  something  passed  to  a  filtering  algorithm  for  it  to  operate  on.  These  terms 
are  often  used  interchangeably  in  the  literature,  but  this  definition  will  distinguish  between  them  here. 

The  measurements  available  from  the  system  include  three  orthogonal  components  of  the  Sun  vector,  three 
orthogonal  components  of  the  magnetic  field  vector,  and  three  orthogonal  components  of  the  rotational 
rate  vector.  Each  of  these  vectors  may  be  expressed  either  in  the  inertial  frame,  or  in  the  body  frame  of  the 
rotating  object.  For  this  implementation,  the  Sun  and  magnetic  field  vector  are  assumed  to  be  available 
from  models  in  the  inertial  frame,  and  are  measured  by  sensors  in  the  body  frame.  The  rotational  rates  are 
handled  differently,  depending  upon  whether  rate  sensors  are  included  in  the  sensor  suite  or  if  the  rates  are 
derived  by  differencing  quaternions  to  get  quaternion  rates  and  then  body  rates.  In  either  case,  the 
rotational  rates  are  expressed  as  body  rates  about  the  body  principle  axes. 

The  observations  that  are  fed  to  the  EKF  or  UKF  portions  of  the  algorithms  consist  of  seven  elements. 
The  Sun  and  magnetic  field  vectors  are  processed  by  the  Gauss-Newton  routine  to  provide  a  best  estimate 
of  the  attitude  quaternion.  The  four  components  of  this  quaternion  are  the  “observations”  upon  which  the 
filters  operate,  not  the  measured  vector  components.  The  remaining  three  observations  are  the  three 
elements  of  the  rotational  rate  vector,  whether  measured  or  derived  by  differencing. 
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It  is  important  to  consider  the  filter  time  step  required  for  successful  estimation  of  the  rocket  rotational 
motion.  Especially  in  light  of  the  rudimentary  dynamic  model  used  by  both  filter  types  to  propagate  the 
motion  forward  one  time  step,  this  time  step  must  be  small  relative  to  the  time  constants  of  the  expected 
motion.  The  highest  frequency  motion  simulated  here  is  the  225  revolution  per  minute  (rev/min)  motion 
about  the  longitudinal  axis  of  the  rocket.  This  is  equal  to  approximately  23.6  rad/sec  which  implies  a  time 
scale  in  the  neighborhood  of  .04  seconds.  If  a  Nyquist  guideline  of  sampling  at  greater  than  or  equal  to 
three  times  the  highest  frequency  of  the  signal  is  used,  then  the  filter  time  step  should  be  no  greater  than 
.013  seconds  to  accurately  capture  the  motion.  In  the  simulation  portion  of  this  work,  to  be  detailed  later, 
data  points  are  simulated  at  a  frequency  at  least  three  times  the  highest  frequency  of  the  expected  motion. 
Likewise,  a  filter  time  step  less  than  or  equal  to  one-third  the  shortest  time  constant  is  used.  In  recognition 
of  the  goal  to  be  able  to  implement  the  final  algorithm  in  real  time,  for  the  baseline  case  where  the 
maximum  rate  of  rotation  is  225  rev/min,  filtering  is  accomplished  at  a  time  step  equal  to  .01  seconds. 
Results  in  Chapter  1 1  illustrate  the  negative  performance  impact  of  filtering  at  a  longer  time  step  than  that 
predicted  by  this  approach. 

9,2*2  Processing  Methods 

Essentially,  four  algorithms  are  developed  for  this  work.  They  may  be  described  as  Gauss-Newton  /  EKF 
/  no  rate  sensors,  Gauss-Newton  /  EKF  /  with  rates  sensors,  Gauss-Newton  /  UKF  /  no  rate  sensors,  and 
Gauss-Newton  /  UKF  /  with  rate  sensors.  In  each  case,  the  Sun  and  magnetic  field  vectors  are  measured  in 
the  body  frame.  At  the  same  time  their  inertial  frame  values  are  determined  by  accessing  models  with  an 
accurate  time  and  position.  The  Gauss-Newton  routine  minimizes  an  error  function  to  determine  the  best- 
fit  transformation  between  the  two  coordinate  frames  and  represents  this  as  an  estimate  of  the  attitude 
quaternion.  Next,  the  four  components  of  this  quaternion,  along  with  the  three  components  of  the 
rotational  rate  vector,  are  provided  as  observations  to  the  EKF  or  the  UKF1  as  the  case  may  be.  This 
filtering  algorithm  then  uses  the  observations,  an  embedded  dynamic  model,  and  a  “memory”  of  past  state 
vectors  to  produce  an  updated  estimate  of  the  rocket  state  vector.  Being  a  sequential  filter,  at  each  step 
only  the  current  estimate  is  maintained,  as  opposed  to  a  batch  method  that  requires  all  past  estimates  to  be 
stored  for  processing  [85].  In  cases  where  rotational  rate  measurements  are  provided  by  a  sensor,  the 
measurements  in  each  axis  are  fed  to  the  filters  as  observations.  In  cases  where  rates  are  not  measured, 
there  is  a  one  step  time  delay  to  allow  for  a  differencing  of  quaternion  components  over  one  time  step. 
This  differencing  produces  a  derived  quaternion  rate  according  to  the  following  equation 

.  qk~qk-\ 

Q= - 

At 


(9.2) 
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where  k  and  k-1  signify  successive  time  steps  and  At  signifies  the  magnitude  of  the  time  step.  Here  each 
of  the  four  components  is  differenced  to  provide  a  four  element  time  rate  of  change  of  the  quaternion. 

This  quaternion  rate  is  then  used  to  calculate  the  three  body  rates  according  to 

4 

O).  =  2  .  (9.3) 

i  7=1  iJ  J 

obtained  by  rearranging  equation  (4.22).  This  effectively  provides  a  noisy  “measurement”  of  the  angular 
rates  expressed  in  the  body  frame.  These  in  turn,  may  be  fed  to  the  filtering  portion  of  the  algorithm  as  an 
observation  of  the  rates  in  the  case  where  no  true  rate  sensor  is  employed. 

9.2.3  Block  Diagram  of  Overall  Process 

Figure  9.1  provides  a  visual  summary  of  the  overall  process.  The  following  sections  will  fill  in  the  detail 
of  how  the  various  blocks  are  implemented.  Tuning  of  the  filters,  or  choosing  the  various  parameters,  will 
be  discussed  in  Chapter  10.  A  discussion  of  the  detailed  results  follows  in  Chapter  11. 


Figure  9.1:  Overview  of  Attitude  Estimation  Algorithm 
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9.3  Sensor  Characteristics 

The  relative  merits  of  the  various  attitude  sensors  available  were  discussed  in  Chapter  6.  For  the 
development  of  this  algorithm,  the  Sun  vector  and  the  magnetic  field  vector  are  used  as  references.  The 
following  sections  describe  the  sensor  characteristics  assumed  for  the  development. 

9.3.1  Sun  Sensor 

Based  on  the  reported  accuracy  from  the  SRP-4  mission,  the  Tokai  University  designed  and  built  Sun 
sensor  is  capable  of  +/-4  degrees  measurement  accuracy  [53].  Since  this  sensor  makes  use  of  relatively 
straightforward  technology  and  manufacturing  techniques,  this  accuracy  should  be  typical  of  what  can  be 
achieved  by  a  low-cost  program.  Therefore,  this  development  assumes  a  standard  deviation,  in  each  of  the 
three  body  axes,  of  1.333  degrees.  In  order  to  achieve  this,  more  than  one  sensor  is  required  in  order  to 
measure  the  angle  in  each  of  the  body  axes.  Additionally,  due  to  view  angle  considerations  generated  by 
the  spinning  motion  inherent  in  sounding  rockets,  additional  sensors  may  be  required  to  provide 
measurements  at  the  frequency  determined  by  the  necessary  filter  time  step.  This  is  a  hardware  design 
issue  beyond  the  scope  of  this  effort. 

9.3.2  Magnetometer 

As  detailed  in  Chapter  6,  a  measurement  accuracy  of  10  degrees  should  be  easily  attainable,  even  in  a  low- 
cost  endeavor.  As  developed  in  Chapter  6,  this  corresponds  to  a  measurement  standard  deviation  of  3.333 
degrees.  This  will  be  the  baseline  measurement  standard  deviation  used  for  the  algorithm  design.  Here,  as 
with  the  Sun  sensor,  the  sensor  must  measure  in  each  of  the  body  frame  axes,  and  the  measurement 
interval  is  assumed  to  be  .01  seconds.  Among  other  things,  the  hardware  design  would  need  to  take  into 
account  sampling  rate  as  well  as  spin  rate  of  the  vehicle.  For  a  magnetometer,  unlike  for  the  Sun  sensor, 
view  angles  will  not  typically  be  an  issue. 

9.3.3  Rate  Gyroscopes 

The  baseline  rate  gyroscopes  used  to  develop  this  algorithm  have  a  sampling,  or  measurement,  rate  of  100 
samples  per  second.  The  measurement  standard  deviation,  in  each  body  frame  axis,  is  assumed  to  be  1 
revolution  per  minute.  This  is  a  very  coarse  rate  measurement,  and  is  in  line  with  what  can  be  expected 
from  the  low-cost  MEMS  gyroscopes  now  available.  The  rate  measurements  described  in  [11]  have  a 
much  lower  standard  deviation,  on  the  order  of  7.4  x  10'5  to  .03  rad/sec,  or  .001  to  .3  rev/min.  Therefore, 
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the  1  rev/min  measurement  accuracy  corresponding  to  a  0.333  rev/min  should  be  easily  attainable,  and  is 
the  baseline  for  this  effort. 

9.4  Dynamic  Modeling  of  Rocket  Motion 

As  discussed  in  Chapter  5,  a  dynamic  model  is  required  for  embedding  in  the  EKF  and  UKF  routines. 
This  model  is  not  a  high  fidelity  model  for  accurate  propagation  of  the  rocket  motion,  but  a  coarse  model 
for  propagating  individual  states  from  one  time  step  to  the  next  as  part  of  the  estimation  process. 

9.4.1  Simplified  Dynamic  Model 

Based  on  the  rationale  laid  out  in  Chapter  5,  a  simplified  dynamic  model  is  used  for  development  of  this 
algorithm.  Figure  5.1,  depicting  the  process  by  which  rotational  rates  are  driven  by  white  noise,  is 
repeated  here  as  Figure  9.2  as  an  aid  to  the  reader.  As  discussed  earlier,  this  model  is  embedded  in  the 
filter  algorithm  and  is  used  to  propagate  the  rotational  rate  states  forward  one  time  step.  Its  suitability 
depends  upon  the  assumption  that  the  inertia  of  the  physical  rocket  prevents  radical  changes  of  rocket 
motion  over  the  duration  of  one  filter  time  step.  As  detailed  in  Section  9.2.1,  the  filter  time  step  used  for 
this  analysis  is  .01  seconds.  This  step  size  is  less  than  one  third  the  time  scale  of  the  highest  frequency  of 
the  expected  rocket  motion.  This  highest  expected  frequency  is  the  225  rev/min  or  23.6  rad/sec  spin  rate 
about  the  longitudinal  axis,  corresponding  to  a  time  scale  of  approximately  .013  sec.  Again,  in  this  figure, 
0)  is  a  3x1  vector  of  angular  rates  co4,  ©2,  and  ©3  in  the  body  frame,  wr  is  a  3x1  vector  of  white  noise 
components  generating  ©b  ©2,  and  ©3,  and  ir  is  a  3x1  vector  of  time  constants  corresponding  to  ©1,  ©2, 
and  ©3. 


Figure  9.2:  White  Noise  Driven  Angular  Rates  (After  Figure  3.1  in  [1 1]) 


58 


9.4.2  Model  Parameters 

There  are  very  few  parameters  necessary  to  implement  this  simplified  model.  The  spectral  characteristics 
of  the  white  noise,  embodied  in  wn  must  be  defined.  Additionally,  the  time  constants,  xr,  must  be 
determined.  For  this  effort,  both  are  solved  for  empirically  and  this  process  will  be  described  in  detail  in 
Chapter  10  during  the  discussion  of  filter  tuning.  Clearly,  such  a  model  lacks  a  measure  of  fidelity,  but 
has  the  great  advantage  of  simplicity,  and  transportability  between  systems.  None  of  the  parameters  is 
unique  to  a  particular  rocket,  but  is  instead  a  function  of  the  motion  itself.  While  not  suitable  for  detailed 
and  accurate  prediction  of  motion,  it  is  sufficient  for  embedding  in  the  EKF  and  the  UKF  for  state 
propagation  across  small  time  steps.  In  this  respect,  the  benefits  of  simplicity  and  transportability 
outweigh  what  is  lost  in  terms  of  model  fidelity. 

9.5  Quaternion-Based  Attitude 

The  attitude  estimation  algorithms  developed  here  rely  on  quaternions  for  their  attitude  representation. 
There  are  numerous  advantages  to  using  this  parameterization,  many  of  which  were  outlined  earlier  in 
Chapter  4.  The  principle  motivation  for  this  application  is  avoidance  of  singularity  issues  and 
minimization  of  the  number  of  calculations.  As  illustrated  in  Figure  9.1,  the  Gauss-Newton  routine 
produces  the  first  estimate  of  the  attitude  quaternion  by  performing  an  error  minimization.  This 
quaternion  estimate  is  normalized  to  magnitude  of  1,  to  insure  that  it  represents  a  pure  rotation.  The 
mechanics  of  the  Gauss-Newton  routine  are  described  in  a  later  section.  This  initial  estimate  of  the 
quaternion  is  then  input  to  the  filtering  routine,  either  EKF  or  UKF-based,  as  a  “measurement”  where  it  is 
further  refined  into  a  better  estimate  of  the  attitude  transformation.  Several  of  the  properties  of 
quaternions  are  utilized  both  in  the  simulation  step  of  this  work  as  well  as  in  the  “differencing”  process  to 
generate  rotational  rate  “measurements”  when  rate  sensors  are  not  used.  While  the  overall  use  of 
quaternions  is  addressed  in  Chapter  4,  the  next  two  sections  highlight  those  properties  most  critical  to  the 
implementation  here. 

9.5.1  Relationship  Between  Quaternions  and  Euler  Angles 

For  reasons  to  be  explained  in  Section  10.1.1,  the  simulation  of  data  for  developing  and  testing  the 
algorithm  relies  on  propagating  Euler  angles  and  then  converting  these  angles  to  a  quaternion.  Therefore 
the  relationship  between  the  two  methods  of  representing  attitude  is  an  important  aspect  of  this  effort. 
This  key  result,  first  presented  in  Chapter  4,  (4.27)  is  repeated  here  as  an  aid  to  the  reader. 
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In  addition  to  this  relationship  between  the  Euler  angles  and  the  attitude  quaternion  components,  the 
relationship  between  body  rates  and  quaternion  rates  is  important  in  terms  of  generating  rotational  rates  to 
input  to  the  filtering  algorithm  when  rate  sensors  are  not  used. 


9.5.2  Relationship  Between  Quaternion  Rates  and  Body  Rates 


For  the  case  where  rotational  rates  are  not  directly  measured,  an  alternative  method  relies  on  deriving  the 
rotational  rates  by  differencing  subsequent  quaternions,  and  then  converting  the  resulting  quaternion  “rate” 
into  a  body-frame  rotational  rate  vector.  In  order  to  do  this,  one  must  have  a  relationship  between  the  two. 
As  presented  in  Chapter  4  as  (4.22),  this  relationship  is 


..0).  . 

V  i 


(9.5) 


where  qj  are  the  components  of  the  attitude  quaternion  and  g>;  are  the  components  of  the  body-frame 
rotational  rate  vector.  The  mechanics  of  deriving  rate  measurements  were  presented  in  Section  9.2. 
Another  way  of  looking  at  this  relationship  between  body  rates  and  quaternions  is  to  combine  the 
simplified  dynamics  model  of  Section  9.3,  the  implementation  of  equation  (4.22)  shown  above,  and  the 
normalization  step.  Again,  the  normalization  is  necessary  to  insure  that  the  quaternion  produces  a  pure 
rotation,  with  no  change  in  magnitude.  Figure  9.3  below  is  a  pictorial  of  the  resulting  process  model 
which  is  used  to  form  the  state  equations  that  are  integrated  to  propagate  the  estimated  states  forward  one 
time  step. 
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Figure  9.3:  Process  Model  Relating  Unit  Quaternion  to  White  Noise  Generated  Angular  Rates 
9,6  Gauss-Newton  Parameter  Optimization 

As  part  of  this  algorithm  to  form  a  best  estimate  of  attitude  motion,  a  Gauss-Newton  error  minimization  is 
used  to  find  the  best  transformation  between  the  inertial  frame  and  the  rotated  body  frame.  Since  an 
attitude  quaternion  is  a  representation  of  this  transformation,  minimizing  the  error  in  the  estimated 
transformation  essentially  provides  the  best  estimate  of  the  attitude  quaternion.  As  demonstrated  by 
Marins  [11],  implementing  such  a  routine  effectively  reduces  the  complexity  of  the  state  equations  for  a 
filtering  routine.  A  traditional  approach  for  implementing  a  Kalman  filter-based  algorithm  would  likely 
start  with  the  formation  of  a  state  vector  made  up  of  three  rotational  rates,  and  then  either  four  quaternion 
components,  three  Euler  angles,  or  nine  elements  of  a  direction  cosine  matrix  to  fully  describe  the 
rotational  motion  of  the  rocket.  Within  the  EKF,  state  equations  must  be  formed  to  allow  propagation  of 
the  states  forward.  These  are  typically  functions  of  the  current  state  vector.  In  a  traditional  formulation, 
the  measured  values  of  the  rates  and  the  components  of  the  two  reference  vectors  would  be  the 
observations.  In  this  case,  once  the  states  were  propagated  forward  one  time  step,  the  predicted 
observations  at  that  time  step  would  need  to  be  calculated  from  the  propagated  states.  As  derived  by 
Marins  [11],  these  functions  are  complex,  nonlinear  expressions  that  do  not  lend  themselves  to  the  real¬ 
time  application  sought  here.  As  will  be  seen  in  a  later  section,  using  the  Gauss-Newton  routine  to 
generate  derived  “measurements”  of  the  attitude  quaternion  greatly  simplifies  these  state  equations,  and 
the  generation  of  the  predicted  observations.  The  outputs,  which  are  compared  to  measurements  to  form 
residuals,  are  typically  some  function  of  the  measured  states.  Using  an  error  minimization  technique,  in 
this  case  Gauss-Newton  error  minimization,  to  determine  an  attitude  quaternion  allows  the  formation  of 
output  equations  that  are  identical  to  the  measured  rotational  rates  and  the  quaternion  components,  thereby 
reducing  the  complexity  of  the  filter  algorithm.  This  not  only  simplifies  the  filter  design,  but  also  yields  a 
corresponding  reduction  in  the  number  of  calculations  and  hence  run  time  required  to  execute  the 
algorithm. 
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9,6.1  Design  of  the  Gauss-Newton  Routine 

Similar  to  the  development  in  [1 1],  the  Gauss-Newton  routine  is  used  here  to  reduce  the  dimension  of  the 
state  vector.  Essentially,  instead  of  using  the  measured  values  of  the  Sun  and  magnetic  field  vectors  as 
“observations”  for  the  filtering  algorithm,  Gauss-Newton  is  used  to  generate  an  attitude  quaternion  which 
serves  as  a  “derived  observation.”  This  condenses  the  six  components  of  the  measured  reference  vectors 
into  four  elements  of  the  quaternion.  This  process  requires  the  Sun  and  magnetic  field  vectors  to  be 
known  in  the  inertial  frame  for  comparison  to  those  measured  in  the  body  frame  of  the  rocket.  This 
assumes  the  availability  of  an  accurate  solar  ephemeris,  an  accurate  magnetic  field  model,  a  good  time 
reference,  and  an  accurate  position  of  the  rocket.  For  this  work,  these  values  are  simulated  as  part  of  the 
controlled  evaluation  of  the  algorithms  designed.  For  a  real-world  implementation,  a  radar  or  GPS 
position  might  be  used  along  with  a  time  reference  to  access  models  and  determine  inertial  Sun  and 
magnetic  field  vectors  at  each  time  step. 


Based  on  this  general  concept,  in  conjunction  with  the  Gauss-Newton  equations  laid  out  in  Chapter  7,  the 
detailed  design  may  be  derived.  The  premise  upon  which  the  choice  of  Gauss-Newton  is  based  is  that 
there  exists  a  quadratic  error  function  that  can  be  minimized.  Here  we  have  two  three  component  vectors, 
the  Sun  vector  and  the  magnetic  field  vector,  that  are  measured  in  the  body  frame  and  known  in  the  inertial 
frame.  The  two  known  vectors  can  be  concatenated  into  a  single  6x1  reference  vector.  Likewise,  the  two 
measured  vectors  can  be  formed  into  a  single  6x1  measurement  vector.  The  goal  is  to  find  the 
transformation  that  takes  one  into  the  other.  If  M(q)  is  related  to  R(q),  the  transformation  between  the 
body  frame  and  the  inertial  frame,  as 


M(q)  « 


R(q) 

0 


0 

R(q) 


(9.6) 


then  the  error  vector  can  be  formed  as  [1 1] 

£(q) 


I  _B 
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where 

e(q)  =  error  vector  (dimension  6x1), 


y ^  =  Sun  and  magnetic  field  reference  vector  in  inertial  frame  at  time  step  k  (dimension  6x1), 

y ^  =  Sun  and  magnetic  field  reference  vector  measured  in  body  frame  at  time  step  k 
(dimension  6x1), 

M  (q)  =  transformation  matrix  composed  from  R,  the  3x3  rotation  matrix  relating  body  frame  to 
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inertial  reference  (dimension  6x6). 

For  this  problem  the  Jacobian  described  by  equation  (7.10)  using  N=1  and  n=4  becomes 
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yk  ,  the  reference  vector  composed  by  concatenating  the  inertial  Sun  and  magnetic  field  vectors,  does  not 
depend  on  the  quaternion  elements.  Therefore,  Jk  reduces  to 
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which  is  dimension  6x4.  In  Chapter  4,  equation  (4.10),  we  saw  that  the  rotation  matrix  /?,  relating  the 
rotated  body  frame  to  the  inertial  frame,  is  defined  as 
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Substituting  R(q)  into  equation  (9.6)  and  multiplying  yields  the  6x1  matrix 
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and  taking  the  first  partial  as  indicated  in  equation  (9.9),  one  gets 
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(9.14) 
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Substituting  these  partials  into  equation  (9.9)  to  form  the  Jacobian  and  then  substituting  the  expression  for 
the  error  vector  from  equation  (9.7)  into  equation  (7.15)  yields 

=$k-ijT  )i_1  jT  uk  *y[  -  >  <9-16> 

This  equation  is  supplied  with  the  last  estimated  quaternion  as  an  initial  guess  and  then  iterated  until  the 

convergence  criterion  is  met  or  until  the  maximum  number  of  iterations  is  reached.  In  this  implementation 

the  convergence  criteria  is  the  mean  square  error  as  determined  from  the  error  vector  described  by 

equation  (9.7),  and  is  shown  below 
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F(q)  -  error  function  to  be  minimized, 
k 

e(q)  =  error  vector, 

=  measurement  vector  in  the  “navigation”  frame, 

M(g)  =  transformation  matrix  from  “body”  to  “navigation”  frame, 

—B 

y^  =  measurement  vector  in  the  “body”  frame. 

The  output  is  the  next  Gauss-Newton  estimated  attitude  quaternion  based  on  minimizing  the  error  in  the 
transformation  from  the  inertial  frame  to  the  body  frame.  The  software  implementing  this  routine  is 
gauss_newton.m  and  is  found  in  Appendix  A. 
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9.6.2  Inputs  to  Error  Minimization  Routine 

Given  the  description  of  the  detailed  routine  design  provided  in  the  last  section,  Figure  9.4  below  outlines 
the  general  process  for  determining  the  attitude  quaternion  estimate. 


Inertial  Sun  Vector 


Inertial  Mag  Vector 


Measured  Sun  Vector 


Measured  Mag  Vector 


Figure  9.4:  Outline  of  Gauss-Newton  Routine 

This  diagram  illustrates  the  inputs  to  the  routine,  and  shows  the  output  to  be  the  estimated  attitude 
quaternion.  As  will  be  demonstrated  later,  this  output,  along  with  the  three  measured  rotational  rates, 
comprise  the  observations  for  the  filtering  algorithm.  This  concludes  the  discussion  of  the  “front  end”  of 
the  algorithm  that  is  common  to  both  the  EKF  and  UKF  based  algorithms.  The  next  section  details  the 
development  of  the  traditional  EKF  based  algorithm. 

9.7  Extended  Kalman  Filter  Design 

Having  discussed  what  inputs  are  available  to  the  filtering  routine  and  how  the  Gauss-Newton  routine 
provides  a  derived  measurement,  the  elements  are  now  in  place  to  look  at  the  detailed  design  of  the 
various  filtering  algorithms.  This  section  addresses  the  traditional  Extended  Kalman  Filter  (EKF) 
approach.  While  the  theory  and  characteristics  have  been  covered  in  previous  chapters,  here  the  detailed 
design  is  reviewed.  The  equations  for  the  various  matrices  can  be  found  in  any  one  of  many  excellent 
references  on  Kalman  filtering  [17],  [68],  [71].  Figure  9.5  provides  an  overview  of  the  entire  EKF 
structure  as  a  prelude  to  how  the  filter  is  implemented  to  include  what  matrices  and  other  quantities  must 
be  defined. 
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z  =  h(x)  x  =  propagated 


Figure  9.5:  Overall  Block  Diagram  of  Extended  Kalman  Filter 


67 


9.7.1  Choice  of  State  Vector 


Designing  the  filtering  algorithm  requires  choosing  what  states  will  be  estimated.  Since  the  goal  is  to 
estimate  the  attitude  of  the  rocket,  things  that  directly  impact  its  rotational  motion  over  time  should  be 
included.  As  discussed  throughout  the  earlier  chapters,  the  attitude  of  the  vehicle  will  be  parameterized 
using  quaternions.  Therefore,  it  makes  sense  to  include  the  four  quaternion  elements  as  states. 
Additionally,  since  the  attitude  is  not  expected  to  be  a  static  orientation,  the  rotational  rates  are  included  as 
states  to  be  estimated.  The  state  vector  is  represented  as 


x  = 


co2 

<°3 

<?3 

L«4j 


(9.18) 


9.7.2  Derivation  and  Definition  of  Filter  Matrices 

As  depicted  in  Figure  9.5,  there  are  a  large  number  of  matrices  that  must  be  defined  for  implementation  of 
the  Extended  Kalman  Filter  (EKF).  The  software  implementation  is  found  in  Appendix  A.  The  two  files 
rates_ekf.m  and  norates„ekf.m  implement  the  EKF  routines  for  cases  where  rotational  rate  measurements 
come  from  sensors  and  where  rotational  rate  “measurements”  come  from  differencing,  respectively.  This 
section  details  the  derivation  of  the  various  equations  and  matrices  coded  in  these  routines. 

9.7.2.1  Initializing  the  State  Vector,  Xq 

The  EKF  filter  routine  requires  an  initial  guess,  or  estimate,  of  the  state  vector  to  begin  processing.  While 
a  simple  solution  is  to  begin  with  all  zeros,  this  risks  exacerbating  the  well-known  issues  concerning  filter 
divergence  due  to  poor  initialization.  Realistically,  at  the  initial  time,  a  reasonable  estimate  of  the  rocket 
attitude  and  rates  should  be  available.  Since  this  work  relies  on  a  simulation  environment,  the  user  is 
prompted  for  the  initial  conditions,  or  the  program  defaults  to  the  actual  initial  conditions  with  a  user 
defined  percent  error.  For  the  results  in  this  work,  a  five  percent  error  was  added  to  the  actual  initial 
values  coming  from  the  simulation  algorithm,  and  these  were  used  to  initialize  the  state  vector.  Since  the 
initial  simulated  rates  are  zero,  the  error  added  is  zero.  The  quaternion  component  errors  are  not  zero. 
This  is  realistic  as  the  states  of  a  rocket  on  the  launch  rail  should  be  relatively  easily  estimated,  with  less 
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error  in  the  rates  than  in  the  quaternion  elements.  In  a  real  world  implementation,  if  a  filter  “restart”  was 
required  during  a  flight,  the  snapshot  provided  by  the  Gauss-Newton  routine  could  be  used  to  reinitialize 
the  filter.  While  divergence  may  occur  for  a  very  poor  initialization,  anything  close  to  the  actual 
orientation  and  rates  should  allow  for  proper  operation. 

9.7.2.2  Initializing  the  Covariance  Matrix,  P 

The  covariance  matrix  is  a  measure  of  the  confidence  in  the  estimate.  In  this  case  the  initial  covariance 
matrix  reflects  the  confidence  in  the  initial  estimate  of  the  state  vector.  This  matrix  is  defined  as 

PXq  =  £[(*o  - *0 ~i0)T]  (9- 19) 

where  E  symbolizes  the  “expectation”  operator  and  this  may  be  interpreted  as  the  squared  error  in  the 
initial  estimate.  In  terms  of  the  program  implemented  here,  this  matrix  is  initialized  with  the  user-defined 
confidence  in  the  initial  estimate,  either  based  on  the  user-defined  error  to  the  actual  initial  conditions,  or 
the  entered  confidence  in  the  user-entered  initial  conditions.  For  software  implementation,  this  matrix  is 
generated  as  a  diagonal  matrix  with  the  squares  of  the  error  corresponding  to  each  initial  state  along  the 
diagonal.  For  the  study  conducted  here,  this  error  in  the  initial  states  is  always  five  percent  of  the  actual 
initial  condition  as  described  in  the  previous  section  Since  this  is  a  covariance  matrix  representing  squared 
error,  it  must  be  positive  definite  by  definition.  Therefore,  a  small  positive  value  is  added  to  each  element 
to  insure  the  matrix  remains  positive  definite  in  the  case  where  the  error  in  an  initial  estimated  state  is 
deemed  to  be  zero. 

9Z7.2.3  Computing  the  Measurement  Noise  Matrix,  R 

The  Measurement  Noise  Matrix,  /?,  gives  a  reflection  of  the  confidence  in  the  accuracy  of  the 
measurements.  It  too  is  a  covariance  matrix.  It  is  defined  as 

R  =  E[vvT]  (9.20) 

where  the  symbol  E  is  again  the  expectation  operator  and  v  is  the  vector  seen  in  (8.2)  representing  the 
white  noise  process  that  corrupts  the  sensor  measurements.  Taking  the  expectation  shown  above,  this 
matrix  may  be  interpreted  as  a  diagonal  matrix  with  the  squares  of  the  sensor  standard  deviation,  or  their 
variances,  along  the  diagonal.  In  the  non-traditional  implementation  used  here,  however,  there  are  actually 
nine  sensor  measurements  coming  from  the  three  rates  and  the  three  components  from  each  reference 
vector.  The  Gauss-Newton  routine  condenses  these  last  six  into  four  “measurements.”  As  a  result,  the 
dimension  of  R  must  be  reduced  from  9x9  to  7x7  in  order  to  be  compatible  for  future  matrix  multiplication 
operations.  Following  the  approach  used  by  Marins  [11]  and  attributed  to  [80],  the  covariance  matrix  for 
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the  quaternion  components  coming  out  of  the  Gauss-Newton  routine,  Rq,  must  be  related  to  the  known 
covariance  matrix  for  the  actual  measurements,  Rsensor •  This  relationship  is  developed  [11]  by  first 
rewriting  equation  (9.16)  as 

h+i  =h~ F^h Ml ~M^k >> k  i •  (9-21) 

Next,  multiplying  through  produces 

qk+l  =qk-  F{qk  )y[  +  F(qk  )M(qk  )yB  .  (9.22) 

As  Marins  points  out,  this  shows  that  the  quaternions  coming  out  of  the  Gauss-Newton  routine  are  a 
function  of  the  previous  estimate  of  the  quaternion.  Equation  (9.22)  can  now  be  decomposed  into  a 
standard  linear  equation  of  the  form 

y  =  Ax  +  B  (9.23) 

where 

B=h-F(h'>yl  (9-24) 

and 

A  =  F(qk)M(qk ).  (9.25) 

This  matrix,  A,  is  computed  at  each  time  step  as  the  Gauss-Newton  routine  reaches  convergence  or  exits 
due  to  reaching  the  maximum  number  of  iterations.  With  the  matrix  A  available,  and  having  the  known 
measurement  covariance  matrix  for  the  measured  vectors,  the  measurement  covariance  for  the  quaternion 
elements  is  found  using  [11] 

Rq  =  AR sensor  ^  *  (9.26) 

This  “measurement”  covariance  matrix  is  then  combined  with  the  measurement  covariance  matrix  of  the 
rate  sensors  to  form  a  7x7  overall  measurement  covariance  matrix. 

For  a  real-world  implementation,  these  sensor  standard  deviations  must  be  determined  and  their  squares 
are  used  to  initialize  the  corresponding  diagonal  elements  of  Rsens0r  in  the  algorithm.  In  the  case  of  the 
implementation  using  rates  derived  from  differencing,  finding  the  standard  deviation  of  these 
“measurements”  is  problematic.  For  the  cases  used  in  this  work,  the  actual  standard  deviation  is  calculated 
and  then  used.  This  information  is  not  available  in  a  real-world  implementation  and,  currently,  the  only 
available  solution  is  to  simulation  the  motion  ahead  of  time,  and  then  use  “typical  values”  for  the  matrix 
initialization.  For  the  cases  using  actual  rate  sensors,  this  is  not  an  issue.  For  the  simulations  run  for  this 
work,  the  standard  deviations  of  all  the  simulated  measurements  are  available  and  the  squares  of  these 
values  are  used  to  initialize  the  Rsensor  matrix. 
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9.7.2.4  Computing  the  State  Transition  Matrix,  0 

The  State  Transition  Matrix,  0 ,  embodies  the  dynamic  equations  needed  for  the  EKF  to  propagate  the 
state  vector  forward  in  time.  This  is  also  the  matrix  that  contains  the  linearization  of  the  nonlinear 
dynamics  in  the  form  of  a  truncated  Taylor  Series  approximation.  The  state  space  representation  of  the 
process  model  in  Figure  9.3,  assuming  no  inputs  other  than  the  white  noise  driving  the  rates,  may  be 
written  as 

3c  =  Fx  +  w  (9.27) 

where  F  is  the  Dynamics  Matrix  representing  the  dynamic  equations  for  the  system,  x  is  the  state  vector 
defined  earlier,  and  w  is  a  vector  containing  random,  zero-mean,  process  noise.  This  process  noise  is 
meant  to  represent  the  known  deficiencies  in  the  dynamic  model.  Based  on  the  process  model,  the 
dynamic  equations  for  this  system  may  be  written  as 

-1  1 

d){  =x{  = - x{+ - wrx 

rrx  Trx 

-1  1 

Try  Try 

-1  1 

^3  =*3  * - *3+ - "Vz 

Trz  Tn 

«1  = *4  =  j  2  2  2  2  (*1*7  -  X2X6  +  X3X5  > 

^\x4  +  x5  +x6  +x7 

h  =i5  =  f  2  o  2  f(xlx6+x2xl  ~X3X4^ 

Hx4  +  x5  +x6  +X1 

h  = k6  =  I  2  2  '~2~2(~XlXS  +  *2X4  +  X3X7  > 

2ix 4  +x5+x6+x7 

<?4  =i7  =  i  2  2  2  "~2^~xlx4  ~x2x5  ~x3x6)  (9.28) 

2p4+x5  +x6+xn 


These  are  the  equations,  less  the  process  noise  terms  in  the  first  three,  that  are  numerically  integrated  to 
propagate  the  states  from  one  time  step  to  the  next.  Clearly  they  are  nonlinear  in  terms  of  the  states  and 
this  is  what  drives  the  use  of  the  EKF,  and  later  the  UKF  approach.  The  EKF  requires  that  this  nonlinear 
system  matrix  be  linearized.  This  is  accomplished  by  taking  partials  of  each  of  the  dynamic  equations 
with  respect  to  each  state  and  its  time  derivative  according  to 
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jc  =  Fx  =>  Ax  =  |  —  \Ax 
dx 


where  the  linearized  Dynamics  Matrix,  F,  is  defined  as 

df{x)\ 


F  =  ■ 


dx 


\x=x 


Taking  these  partials  yields  the  following  non-zero  entries  for  F 
F(  U)  =  - 
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The  continuous  time  State  Transition  Matrix,  0,  is  defined  as 

Ft 

0(0  =  ^ 

and  the  discrete  form  seen  in  (8.4)  is  generated  by  substituting  the  filter  time  step,  Ts ,  for  t. 
While  eFt  may  be  represented  as  an  infinite  Taylor  Series  expansion  yielding 


(9.32) 


Ft  (Ft) 

O(0  =  e  =  l  +  Ft  + - — — 

2/ 


(Ft)" 

+  ...  + - + 

n! 


(9.33) 


this  is  not  suitable  for  implementation.  Unlike  in  a  linear  Kalman  filter,  where  the  state  transition  matrix  is 
actually  used  to  propagate  the  states  forward  one  time  step  through  matrix  multiplication,  in  the  EKF,  this 
matrix  is  used  only  to  facilitate  the  calculation  of  the  Kalman  gains.  The  states  are  actually  propagated  via 
numerical  integration  of  the  nonlinear  dynamic  equations.  Given  this  usage,  the  infinite  series  is  truncated 
to  two  terms,  as  additional  terms  do  not  generally  improve  performance  [17].  This  may  be  attributed  to 
the  fact  that,  for  sufficiently  small  time  steps,  the  first-order  linearization  is  valid.  Therefore,  the  discrete 
state  transition  matrix  is  approximated  as 

PT 

<&,  =  e  ~  I  +  FT  .  (9.34) 

k  s 

Substituting  the  values  for  F  derived  above  and  multiplying,  the  non-zero  elements  of  0  are 
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d>(7,3  )  =  —LTs 
dx3 
dxn 

0(7,4)  =  ~~TS 
ox^ 

0(7,5)  =  Ts  (9.35) 

dx5 

0(7,6)  =  -H LTs 
dx6 

^Xn 

$(7,7)  =  1 +  -J-TS 

OX'j 

The  values  of  the  partials  are  those  derived  in  equation  (9.31)  above. 

9.7.2.5  Computing  the  Process  Noise  Matrix,  Q 

The  Process  Noise  Matrix,  Q ,  is  meant  to  reflect  the  confidence  in  the  overall  system  model.  It  is  defined 
as 

T 

Q(t)  =  E[ww  ]  (9.36) 

where  E  is  the  familiar  expectation  operator  and  w  is  the  vector  representing  the  process  noise  in  the  state- 
space  system  equation  (8.2).  The  continuous  time  Q(t)  is  again  a  covariance  matrix  that  is  meant  to  reflect 
the  uncertainty  in  the  dynamic  model  of  the  system.  The  determination  of  this  matrix,  while  seemingly 
straightforward,  is  probably  the  most  difficult  to  accomplish.  This  stems  from  the  difficulty  in  putting 
numbers  to  uncertainty  in  the  model.  It  is  initialized  in  this  case  as 

s°\  0  0  0  0  0  0 

0  <t>s<T^2  0  0  0  0  0 

0  0  <1 >sa^  0  0  0  0 

0  0  0  0  0  0  0 

0  0  0  0  0  0  0 

0  0  0  0  0  0  0 

0  0  0  0  0  0  0 

Here,  the  sigmas  are  the  estimated  standard  deviation  of  the  rate  measurements  in  each  axis  and  <X>S  is  a 
process  noise  tuning  parameter  added  to  account  for  uncertainty  in  the  system  model.  <hs  is  unrelated  to 
the  state  transition  matrix  <£k,  and  determination  of  the  value  for  this  tuning  parameter  is  discussed  in 
Chapter  10..  The  discrete  Qk  is  calculated  during  each  iteration  of  the  EKF  as 
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T  T 

Qk  =  J0S  <&(r)0(f)<&  (r)dt.  (9.38) 

Substituting  <Z>  from  Section  9. 7.2.5,  integrating,  and  simplifying  yields  the  following  elements  for  Qk 
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(9.39) 


Again  the  values  of  the  partials  in  these  equations  were  determined  in  the  derivation  of  F  in  equation 
(9.31). 


9J7.2.6  Computing  the  Covariance  Matrix  Before  the  Update,  M 


The  covariance  matrix  before  the  update,  M,  is  calculated  as 

T 

M  =  4>/M>  +  <2,  . 

k 


(9.40) 
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All  of  these  constituent  matrices  have  been  derived  above  with  the  exception  of  P,  the  covariance  matrix 
after  the  update.  While  the  process  by  which  this  matrix  is  initialized  was  addressed  in  Section  9.7.2.2,  the 
derivation  of  the  recurring  matrix  will  be  addressed  in  an  upcoming  section. 

9.1. 2H  Propagating  the  State  Vector  Forward,  X 

In  the  case  of  a  nonlinear  system  such  as  this,  the  state  vector  cannot  be  propagated  forward  in  time  using 
the  state  transition  matrix  as  would  be  the  case  with  a  linear  system.  Instead,  equations  (9.28)  are 
numerically  integrated  to  propagate  the  states  from  one  step  to  the  next.  In  this  implementation,  a  one-step 
Euler  integration  scheme  is  used. 

9.1.2.%  Calculating  the  Projected  Observations,  Z 

In  a  general  implementation  of  the  EKF,  the  projected  observations  are  a  function,  sometimes  a  nonlinear 
function,  of  the  system  states: 

z  =  h(x).  (9.41) 

Because  of  the  simplification  gained  by  using  the  Gauss-Newton  error  minimization  routine,  in  this 
implementation  the  projected  observations  are  exactly  equal  to  the  projected  states,  or 

z  =  x.  (9.42) 


9.7.2.9  Calculating  the  Measurement  Matrix,  H 


Typically,  for  a  nonlinear  system  whose  states  are  estimated  by  an  EKF,  the  Measurement  Matrix,  //,  is  a 
linearization  of  the  nonlinear  function  of  the  states,  h(x) .  Again,  because  the  projected  observations  are 
simply  the  projected  states,  in  this  case  the  measurement  matrix  is 


H  = 


dh(x ) 


dx 


_  77x7 

x  =  x 


(9.43) 


or  simply  equal  to  the  identity  matrix. 


9.7,2.10  Computing  the  Kalman  Gain  Matrix,  K 

The  Kalman  gain  is  calculated  at  each  time  step  as  a  function  of  the  matrices  we  have  already  defined. 
The  expression  for  this  gain  matrix  is 


K  =  MHT  [HMHT  +  R] 


(9.44) 
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9.7.2.11  Computing  the  Covariance  Matrix  After  the  Update,  P 

Once  the  Kalman  gain  matrix  is  available,  the  covariance  matrix  after  the  update,  P ,  may  be  calculated. 
The  expression  used  to  do  this  is 

P  =  {i~KH)m.  (9.45) 

In  essence,  this  provides  a  measure  of  how  confident  the  filter  is  in  its  estimate  at  each  step. 

9.7.2.12  Computing  the  New  State  Estimate,  X 

Finally,  the  state  estimate  is  created  by  the  filter.  Here,  the  filter  combines  the  state  vector  that  has  been 
propagated  forward  by  numerically  integrating  the  nonlinear  dynamic  equations  with  a  weighted  residual 
formed  by  differencing  the  observation  with  the  projected  observation  and  weighting  it  with  the  Kalman 
gain.  In  this  manner,  the  EKF  produces  its  estimate  of  the  state  vector  at  the  next  time  step.  The  equation 
used  to  calculate  this  estimate  is 

x  =  x  +  K(z*  -  z)  •  (9.46) 

where  z*  is  the  measured  observation  from  the  sensors,  or  in  this  case  from  the  rate  sensors  and  from  the 
Gauss-Newton  routine  in  the  case  of  the  quaternion  components. 

Once  this  estimate  is  formed,  the  cycle  begins  again  for  the  next  time  step  as  depicted  in  Figure  9.5.  This 
entire  process  is  implemented  in  the  files  norates_ekf.m  and  rates_ekf.m  found  in  Appendix  A. 

9.8  Unscented  Kalman  Filter  Design 

The  choice  of  the  Unscented  Kalman  Filter  (UKF)  as  the  most  promising  estimation  algorithm  was 
detailed  in  Chapter  8.  Figure  9.6  gives  a  pictorial  view  of  the  steps  for  implementing  this  routine.  While 
the  UKF  is  a  fairly  recent  development,  the  general  equations  can  be  found  in  several  good  sources  [67], 
[68],  [69].  As  an  aid  to  the  reader,  the  steps  that  differ  from  the  traditional  EKF  are  lightly  shaded.  The 
software  implementation  of  the  algorithm  described  here  is  found  in  Appendix  A.  The  two  files 
rates_ukf.m  and  norates_ukf.m  implement  the  UKF  routines  for  cases  where  rotational  rate  measurements 
come  from  sensors  and  where  rotational  rate  “measurements”  come  from  differencing,  respectively.  This 
section  details  the  derivation  of  the  various  equations  and  matrices  coded  in  these  routines. 


Nonlinear  system  dynamics  model 
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9.8.1  Choice  of  State  Vector 


As  with  the  EKF  described  in  Section  9.7,  the  state  vector  to  be  estimated  must  describe  the  rotational 
motion  of  the  vehicle.  The  UKF  will  be  implemented  to  estimate  the  rotational  rates  that  describe  the 
dynamic  motion  and  the  quaternion  elements  that  provide  the  attitude  transformation  at  each  time  step. 
Therefore,  the  same  state  vector  is  used  for  both  the  EKF  and  the  UKF  and  is  repeated  below. 


x 
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O) tj 

*1 

«3 

*4 


(9.47) 


9.8.2  Derivation  and  Definition  of  Filter  Matrices 

Many  of  the  matrices  derived  for  the  UKF  are  similar,  and  sometimes  are  identical,  to  those  used  in  the 
EKF-based  algorithm.  Since  these  were  defined  in  detail  in  Section  9.7,  only  the  differences  will  be 
elaborated  on  here. 


9.8.2.1  Initializing  the  State  Vector,  X0 

The  UKF  filter  routine  also  requires  an  initial  guess,  or  estimate,  of  the  state  vector  to  begin  processing. 
Realistically,  at  the  initial  time,  a  reasonable  estimate  of  the  rocket  attitude  and  rates  should  be  available. 
Since  this  work  relies  on  a  simulation  environment,  the  user  is  prompted  for  the  initial  conditions,  or  the 
program  defaults  to  the  actual  initial  conditions  with  a  user  defined  percent  error.  The  initialization  of  the 
state  vector  in  this  work  is  always  accomplished  using  the  actual  initial  states  plus  a  five  percent  error  as 
described  for  the  EKF  in  section  9.7.  Also  as  with  the  EKF  routine,  if  a  filter  “restart”  was  required  during 
a  real  world  flight,  the  snapshot  provided  by  the  Gauss-Newton  routine  could  be  used  to  reinitialize  the 
filter. 

9.8.2.2  Initializing  the  Covariance  Matrix,  P 

The  covariance  matrix  is  a  measure  of  the  confidence  in  the  estimate.  As  with  the  EKF,  this  matrix  is 
defined  as 
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%  =E[(x0-xq)(xq-x0)T]  (9.48) 

where  E  symbolizes  the  “expectation”  operator.  For  software  implementation,  this  matrix  is  generated  as 
in  the  same  manner  described  in  section  9.7,  as  a  diagonal  matrix  with  the  squares  of  the  error  in  the  initial 
states  along  the  diagonal. 

9,8.23  Computing  the  Measurement  Noise  Matrix,  R 

The  Measurement  Noise  Matrix,  /?,  gives  a  reflection  of  the  confidence  in  the  accuracy  of  the 
measurements.  It  is  defined  as 

R  =  E[vvT }  (9.49) 

where  the  symbol  E  is  again  the  expectation  operator  and  v  is  again  the  vector  from  (8.2)  which  represents 
the  white  noise  process  that  corrupts  the  sensor  measurements.  Given  the  expectation  operator  in  (9.49), 
this  matrix  may  be  interpreted  as  a  covariance  matrix  of  the  measurement  errors. 

As  described  in  9.7.2.3,  the  Gauss-Newton  routine  produces  a  matrix,  A,  at  each  time  step  that  relates  the 
known  measurement  standard  deviations  to  the  derived  quaternion  component  standard  deviations. 

Rq  =  ARsenSorA  .  (9.50) 

This  4x4  diagonal  “measurement”  covariance  matrix  is  then  combined  with  the  measurement  covariance 
matrix  of  the  rate  sensors  to  form  a  7x7  diagonal  overall  measurement  covariance  matrix.  For 
implementation  purposes  the  sensor  standard  deviations  are  used  to  initialize  the  diagonal  elements  of 
Rsenson  subject  to  the  same  constraints  described  in  Section  9.7.23. 

9.8.2.4  Computing  the  Process  Noise  Matrix,  Q 

As  is  clear  in  Figure  9.6,  the  Process  Noise  Matrix,  Q ,  does  not  change  once  it  is  initialized.  For  the  UKF, 
the  continuous  time  Q(t),  is  defined  as 

T 

<2(0  =  E[ww  )  (9.51) 

where  E  is  the  familiar  expectation  operator  and  w  is  the  error  vector  in  the  state-space  system  equation 
(8.2).  Since  the  process  noise  matrix  reflects  the  confidence  in  the  system  model,  and  lacking  any 
significant  rationale  to  increase  one  element  over  another,  for  the  UKF  implemented  here  the  diagonal 
elements  of  Q(t)  are  all  initialized  with  the  square  of  the  user  entered  process  noise  tuning  parameter,  Os. 
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L  *  J 

Although  the  process  noise  is  often  determined  experimentally,  as  it  will  be  here,  Zarchan  points  out  that 
“a  good  starting  point  is  that  the  amount  of  process  noise  that  a  Kalman  filter  requires  should  reflect  our 
lack  of  knowledge  of  the  real  world  [17].”  He  elaborates  that,  “One  method  for  determining  process  noise 
is  to  square  the  uncertainty  in  our  expected  initial  error  in  the  estimate  and  divide  by  the  amount  of 
filtering  time.”  For  this  work,  the  optimum  amount  of  process  noise  will  be  determined  empirically.  The 
actual  “tuning”  process  for  this  parameter  is  described  in  Chapter  10. 


9.8.2.5  Calculating  the  Sigma  Points,  X( 

As  described  earlier,  a  number  of  “sigma  points”  must  be  calculated.  These  points  essentially  form  a 
“cloud”  of  points  that  are  then  propagated  forward  as  part  of  the  estimation  process.  At  each  time  step  the 
sigma  points,  Xh  are  calculated  as  [69], 


1  +  ^Pk- 1  *Jt-l  ~  fo-l  1 

(9.53) 

,  oo  and  y  is  defined  as 

y=^L+A 

(9.54) 

where  L  is  the  dimension  of  the  random  variable,  set  equal  to  the  dimension  of  the  state  vector  to  be 
estimated,  or  7.  Wan  and  van  der  Merwe  [68]  express  the  2L+1  sigma  points  this  way 

Xo=* 

Xi  =x  +  (J(L+Z)Px)i,i  =  1  (9.55) 

Xi  =x-(^(L+^)iJ  =  L+l,...,2L 

where  they  note,  for  their  implementation,  that  (^(L  +  A)PX)-  is  the  ith  column  of  the  matrix  square  root 

which  in  their  approach  is  determined  using  a  lower-triangular  Cholesky  factorization.  In  both  cases,  X  is 
a  composite  scaling  factor  defined  as  [68] 
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A  =  a2(L+K)-L.  (9.56) 

‘The  constant  a  determines  the  spread  of  the  sigma  points  about  x  and  is  usually  set  to  a  small  positive 
value  (e.g.,  10'4<a<l).  The  constant  k  is  a  secondary  scaling  parameter  which  is  usually  set  to  3 -L  ... 
[68].”  Further  discussion  of  the  selection  of  these  parameters  is  found  in  Chapter  10. 


9.8.2.6  Propagating  the  Sigma  Points  Forward  in  Time,  X\ 


Once  the  “cloud”  of  sigma  points  is  calculated,  it  must  be  propagated  to  the  next  time  step.  This  is 
accomplished  by  “instantiating”  each  point  through  the  nonlinear  function.  From  an  implementation 
standpoint,  this  means  numerically  integrating  the  nonlinear  state  dynamic  equations  forward  one  time 
step  to  get  a  transformed  “cloud”  of  points  at  the  next  time  step.  The  state  dynamic  equations  are 
determined  from  the  process  model  depicted  in  Figure  9.4  and  are  repeated  here  for  continuity: 
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(9.57) 


In  the  case  of  the  UKF  implemented  here,  the  instantiation  of  the  sigma  points  is  accomplished  using  a 
one-step  Euler  integration  routine  with  a  step  size  of  .01  sec.  The  selection  of  this  step  size  will  be 
discussed  in  Chapter  10.  This  helps  to  minimize  the  time  cost  associated  with  propagating  so  many  points 
through  this  numerical  process. 
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9*8.2.7  Computing  the  Predicted  Mean  for  Each  State,  x 


Once  the  set  of  transformed  sigma  points  is  available,  the  mean  state  vector  at  the  new  time  step  may  be 
calculated.  This  is  accomplished  by  taking  a  weighted  average  of  the  transformed  points,  ;£\.  The 


mathematical  expression  for  this  is 


2  L 


(9.58) 


;=o 


where,  again,  the  L  is  the  dimension  of  the  state  vector  as  established  earlier.  The  weights,  Wj,  are 
determined  using  [68] 


W, 


(m)  _ A 


0 


L  +  A 


w[m)  = — l. — 

•  2(L  +  A) 


(9.59) 


2  L 


where  L  and  X  are  as  previously  defined. 


9.8.2.8  Computing  the  Covariance  Matrix  Before  the  Update,  M 


The  covariance  matrix  before  the  update,  M,  is  found  using 

M  =  2i^c\xi  -x)hi  -*Y  +Q 


i= O' 


(9.60) 


where  L  is  again  the  dimension  of  the  state  vector,  Q  is  the  process  noise  matrix  and  are  the  propagated 

_ 

sigma  points.  X  is  the  projected  mean  state  vector.  Wf  are  again  weights  which  are  found  using  [68] 

2 

+  P 

(9.61) 


tel  X  2 

WA*  = - +  \  +  a  +p 

0  L  +  A 


w.(c)  =w(m)  = - - - ,  i  =  1,...,2L 

1  1  2(L+A) 

L,  X,  and  a  are  as  previously  defined  and  “P  is  used  to  incorporate  prior  knowledge  of  the  distribution  of  x 
(for  Gaussian  distributions,  P=2  is  optimal)  [68].” 


9.8.2.9  Augmenting  the  Sigma  Point  Matrix, 

As  depicted  in  Figure  9.6,  the  next  step  in  a  UKF-based  algorithm  is  to  augment  the  set  of  projected  sigma 
values  to  account  for  the  process  noise  covariance,  Q.  There  are  two  principle  means  to  accomplish  this 
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[68].  The  first,  used  here,  augments  using  additional  points  derived  from  the  matrix  square  root  of  the 
process  noise  covariance  and  requires  setting  L=2L  and  recalculating  the  weights  W,.  A  second  method 
augments  by  redrawing  an  entire  new  set  of  L  points.  While  this  results  in  fewer  points,  and  therefore  less 
propagation  time,  it  also  risks  losing  information  that  may  have  been  captured  by  the  original  set  of  points 
that  were  then  propagated  forward.  In  this  implementation,  the  first  method  is  used  for  the  sake  of 
accuracy.  If  processing  time  becomes  an  issue  in  a  real-time  implementation,  the  second  method  should 
be  further  investigated. 


The  augmentation  points  added  to  the  original  set  of  transformed  points  are  determined  by 

Xi  =J0  +  (V(^+^)fi)i,/  =  ^+ 1 . \l' 

Xi  =  Z0  - (V (£' + X)Q ) j 'i  =  \L'+ 1-"2Z/ 


(9.62) 


where  L’=2L,  and  as  before,  V Z/+/T  =  /  and.  is  the  ith  column  of  the  matrix  square  root  determined 

by  the  lower  triangular  Cholesky  factorization.  The  parameter  X *  must  be  recalculated  based  on  the  new 
values  for  L\a  \  and  k*  due  to  the  augmenting  of  the  sigma  points. 

X,  =  a,2(L,+  K/)-L'  (9.63) 


a’,  and  k’  are  based  on  the  same  criteria  used  before  augmenting  the  sigma  point  matrix.  The  weights  are 
then  recalculated  based  on  the  parameters  for  the  augmented  set  according  to 
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(9.64) 


L'  +  X 


+ 1  +  a'2  +  P' 


W, 


r(c)  =  w(m)  = - 1 -  ■  _  12L' 

1  1  2  {L'+X) 


where  ^’=2  is  optimal  for  a  Gaussian  distribution  [68].  These  new  weights,  based  on  the  augmented  sigma 
set,  are  then  used  for  future  calculations  associated  with  the  predicted  observations  and  covariance 
matrices. 
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9,8.2.10  Propagating  Prediction  Points  Forward  in  Time,  Zf- 


Once  the  augmented  set  of  sigma  points,^*  ,  is  available,  each  vector  is  “instantiated”  through  the 
observation  model 

Z;=/i|y]  (9.65) 

to  get  the  predicted  “observation  cloud”  based  on  the  propagated  sigma  set.  The  function  h  is  typically  a 
nonlinear  function  of  the  state  vector  that  has  to  be  numerically  propagated.  Here,  as  with  the  EKF,  the 
simplification  due  to  using  the  Gauss-Newton  routine  becomes  apparent.  In  this  implementation,  the 
predicted  observations  are  exactly  identical  to  the  projected  states.  Therefore,  in  this  case, 

Zi=zi  (9.66) 

and  no  further  propagation  is  necessary,  thereby  saving  on  computation  time. 


9.8.2.11  Computing  the  Predicted  Observations,  Z 


The  predicted  observations,  z ,  are  calculated  as  a  weighted  sum  using  the  equation 

2  V  2  V  (m)'  ' 

z .  =  I  wm)  z,  =  z  w>m)  Xi  . 

I  1=0  1  1  1=0  1  1 


9.8.2.12  Calculating  the  Innovation  Covariance,  Pu 


The  Innovation  Covariance  Matrix  is  calculated  as  follows 

2  V 


rzz 


%  wic)  fa -ilhi -ij  *R 


In  this  case,  since  Z-  =  » 


(9.67) 


(9.68) 


(9.69) 


where  R  is  the  measurement  noise  covariance  matrix  derived  earlier. 
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9.8.2.13  Calculating  the  Cross  Correlation  Matrix,  Pxz 

The  cross  correlation  matrix  ,  Pxz>  is  also  based  on  the  “augmented  weights”  and  is  found  using 

rxz-%w[c)i?!-s,h!-hf 

or  in  this  special  case. 


(9.70) 


(9.71) 


9.8.2.14  Computing  the  Kalman  Gain  Matrix,  K 
The  Kalman  gain  matrix  is  computed  using 

K  =  P„Pa~\  (9.72) 


9.8.2.15  Computing  the  Covariance  Matrix  After  the  Update,  P 

P,  the  covariance  matrix  after  the  update,  is  a  function  of  the  Kalman  gain,  the  covariance  before  the 
update,  and  the  innovation  covariance.  The  expression  for  calculating  it  is 

P  -  M  -  KP '  KT  .  (9.73) 


9.8.2.16  Computing  the  New  State  Estimate,  JC 

Finally,  the  goal  is  reached  and  the  updated  estimate  of  the  state  vector  is  computed  using 

x  =  x-K(z-z*).  (9.74) 

__  __  * 

Here,  x  is  the  projected  state,  z  is  the  projected  observation,  z  is  the  “measurement”  from  the  sensors, 

and  K  is  the  Kalman  gain  matrix. 
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10.0  Tuning  of  the  Algorithm  and  Validation  of  Performance 

With  a  firm  grasp  of  the  design  of  the  EKF  and  UKF  algorithms  in  hand,  it  is  now  time  to  describe  the 
data  each  will  operate  on  and  how  each  of  the  parameters  required  to  execute  the  algorithms  is  determined. 
This  chapter  addresses  both  these  topics  as  well  as  providing  examples  of  each  algorithm’s  performance. 

10.1  Computer  Simulation  of  True  Motion 

Due  to  a  paucity  of  real,  high  fidelity  attitude  data  for  a  rocket,  the  data  for  development  and  test  of  these 
algorithms  is  simulated.  Both  the  simulation  and  the  algorithms  are  programmed  using  the  software 
package  MATLAB.  The  advantage  of  using  simulation  data  is  that  the  absolute  true  values  are  known. 
These  values  are  corrupted  to  provide  simulated  measurements,  and  then  filtered  in  hopes  of  returning  to 
the  true  data,  which  is  available  for  comparison.  When  using  real  data,  the  truth  is  seldom  known,  and  at 
best  the  experimenter  is  left  to  compare  with  data  that,  hopefully,  is  measured  by  a  system  with  equal  or 
better  accuracy  than  that  being  tested.  The  disadvantage,  of  course,  is  that  the  simulation  may  not 
adequately  reflect  the  real  world.  The  simulation  tool  developed  here  allows  the  user  to  generate  certain 
body  rate  profiles,  which  are  then  translated  into  rocket  attitude  motion.  Other,  more  complex,  profiles 
can  also  be  developed  off-line  and,  with  minor  manipulation  of  the  script  attitude_sim.m,  input  to  the 
overall  algorithm  for  testing.  The  profiles  included  are  intended  to  simulate  motion  representative  of  that 
expected  in  a  sounding  rocket.  They  are  not  intended  to  provide  a  high  fidelity  simulation  of  exact  rocket 
motion,  which  is  extremely  complex.  This  approach  assumes  that  if  the  attitude  estimation  algorithm  can 
successfully  process  the  motions  simulated  here,  it  should  be  able  to  successfully  estimate  attitude  from 
real  sensor  data.  The  next  sections  describe  the  methodology  used  to  produce  simulated  data  for  algorithm 
development  and  testing. 

10.1.1  Methodology  for  Simulating  Motion 

The  following  diagram  depicts  the  process  by  which  the  true  motion,  and  measurement  of  that  motion,  is 
simulated.  The  user-interactive  code  to  accomplish  the  steps  shown  below  is  in  the  MATLAB  file 
attitude_sim.m  and  is  found  in  Appendix  A.  The  desired  rotational  rate  profile  indicated  in  the 
“initialization”  block  in  Figure  10.1  is  chosen  by  the  user  while  running  attitude_sim.m.  The  choices 
consist  of  fixed  rates  about  each  axis,  a  linearly  ramped  profile,  an  exponential  profile  that  asymptotically 
approaches  a  final  value,  or  a  step/pulse  profile.  The  profiles  are  implemented  in  the  programs 
omegagenjix.m,  omegagen_ramp.m,  omegagen_exp.m  and  omegagen_step.m  respectively. 
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Initialization 


Figure  10. 1 :  Generation  of  Simulated  Data  and  Measurements 

These  are  called  by  attitude_sim.m  based  on  user  input,  and  the  code  itself  may  be  found  in  Appendix  A. 
In  each  case,  the  user  is  able  to  define  a  number  of  parameters,  and  the  software  generates  body  rates  at 
each  time  step  for  use  by  attitude_sim.m. 
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From  the  diagram,  it  may  be  seen  that  the  “true”  motion  is  propagated  forward  by  a  numerical  integration 
of  the  Euler  equations,  making  use  of  the  user  defined  rate  profile.  In  this  implementation  a  second-order 
Runge-Kutta  integration  scheme  is  used  to  integrate  equations  (5.3).  This  propagation  of  the  motion  could 
also  be  done  by  integrating  a  quaternion  rate  profile,  using  equation  (4.22).  However,  since  the  goal  is  to 
generate  estimates  of  the  attitude  quaternion,  and  evaluate  the  results  of  an  attitude  quaternion  filter, 
propagating  the  Euler  angles  and  then  converting  the  propagated  value  to  a  quaternion  provides  some 
separation  in  methods.  This  in  turn  gives  more  confidence  that  the  filter  is  truly  estimating  the  quaternion 
from  the  measurements  and  not  somehow  reiterating  what  it  has  been  given.  This  same  approach  was 
used  in  [11].  Of  course,  a  complication  associated  with  the  use  of  Euler  angles  is  the  occurrence  of 
singularities  at  certain  orientations.  While  the  code  includes  protections  against  “divide  by  zero,”  etc.,  the 
user  should  be  aware  of  the  possibility  of  these  singularities  and  investigate  any  aberrations  in  the 
simulated  motion  with  these  in  mind. 

Once  the  Euler  angles  are  propagated  to  the  next  time  step,  equation  (4.27)  is  used  to  calculate  the  “true” 
propagated  quaternion  corresponding  to  these  angles.  The  propagated  Euler  angles  are  also  used  to 
calculate  a  Direction  Cosine  Matrix  (DCM)  using  equation  (4.1).  This  DCM  is  used  to  rotate  the  reference 
vector  defined  in  the  inertial  frame  to  one  defined  in  the  body  frame,  producing  what  a  perfect  sensor  suite 
should  measure.  Next,  both  the  rotated  reference  vector  and  the  rotational  rates  for  a  given  time  step  have 
random  noise  added  to  simulate  noisy  measurements  at  that  time  step.  The  random  noise  is  Gaussian  with 
a  standard  deviation  equal  to  that  defined  by  the  user. 

To  summarize,  attitude_sim.m  produces  arrays  holding  the  simulated  “true”  quaternion,  noisy  Sun  sensor 
and  magnetometer  measurements,  and  noisy  rotational  rate  measurements  for  each  time  step.  These  array 
elements  are  then  read  one  at  a  time  by  the  attitude  determination  algorithm  to  simulate  the  sequential 
availability  of  the  data. 

10.1,2  Single  90  Degree  Rotation 

To  demonstrate  the  correct  function  of  the  data  simulation  implemented  in  attitude_sim.m,  several  simple 
cases  are  run.  These  are  cases  which  can  be  easily  visualized,  yet  some  present  situations  which  would 
cause  difficulties  due  to  singularities.  The  first  is  a  single  rotation  of  90  degrees  about  one  of  the  principle 
axes.  While  rotations  about  each  axis  were  investigated,  a  positive  rotation  about  the  “z”  axis  is  discussed 
in  this  section.  This  rotation  is  easily  visualized,  and  corresponds  to  an  Euler  angle  sequence  of  v|/=90°, 
0=0°,  <p=0°,  or  equivalently,  0°,  0-0°,  (p=90°.  Following  the  rotation,  the  rotated  “x”  axis  points  in  the 
direction  of  the  unrotated  “y”  axis  and  the  rotated  “y”  axis  points  in  the  direction  of  the  original  “-x”  axis. 
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The  “z”  axis  remains  unchanged  since  this  is  a  single  rotation  about  the  “z”  axis.  The  results  of  simulating 
this  motion  using  attitude_sim.m  are  shown  in  Figure  10.2  below.  Keep  in  mind  that  the  body  frame  is 
rotating  according  to  the  Euler  sequence  described  above.  As  a  result,  the  (1, 0,  0)  inertial  vector,  depicted 
here  as  the  Sun  vector,  appears  in  the  rotated  frame  to  have  the  coordinates  (0,-1, 0),  as  expected.  Also  as 
expected,  the  (0,  0,  1)  inertial  vector,  depicted  here  as  the  magnetic  field  vector,  has  unchanged 
coordinates  in  the  rotated  frame. 


3-D  plot  of  inertial  and  rotated  sun  and  mag  field  vectors  and  rotation  quaternion 


Figure  10.2:  Single  90  Degree  Rotation  as  Depicted  by  Attitude_sim.m 

Using  equation  (4.13),  the  corresponding  attitude  quaternion  is  q  =  (0,  0,  0.7071,  0.7071).  Figure  10.3 
below  demonstrates  that  attitude_sim.m  produces  this  expected  result.  Finally,  as  described  above,  this 
rotation  corresponds  to  the  Euler  angle  sequence  \|/=0°,  0=0°,  <p=90°.  As  seen  in  Figure  10.4, 
attitude_sim.m  produces  these  results  as  well.  The  single  90  degree  rotation  performance  is  similar  for 
rotations  about  either  of  the  other  two  axes. 


time  (sec) 


Figure  10.3:  Quaternion  Elements  for  Single  90  Degree  Rotation  as  Produced  by  Attitude_sim.m 


Figure  10.4:  Euler  Angles  for  Single  90  Degree  Rotation  as  Produced  by  Attitude_sim.m 
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10.1.3  Two  Successive  90  Degree  Rotations 

The  last  section  demonstrated  the  results  of  the  easily  visualized  single  90  degree  rotation  about  the  third 
axis.  In  this  section,  similar  results  are  presented  for  the  case  of  a  90  degree  rotation  about  the  “y”  axis 
followed  by  a  90  degree  rotation  about  the  “z”  axis.  Figure  10.5  below  illustrates  the  inertial  (1,  0,  0) 
“Sun”  vector  as  seen  from  the  rotated  frame.  Its  coordinates  are  (0,  0,  1)  as  expected.  Similarly,  the 
inertial  (0, 0, 1)  “mag  field”  vector  is  represented  as  (0,  1, 0)  in  the  rotated  frame.  This  also  is  as  expected. 
In  the  figures  in  this  section,  the  motion  is  simulated  at  a  .01  sec  time  step,  but  only  every  100th  point  is 
plotted  for  clarity. 


3-D  plot  of  Inertial  and  rotated  sun  and  mag  field  vectors  and  rotation  quaternion 


Figure  10.5:  Successive  90  Degree  Rotations  as  Depicted  by  Attitude_sim.m 

Again,  based  on  equation  (4.13),  the  rotation  quaternion  for  the  first  rotation  is  (0,  .7071, 0,  .7071)  and  for 
the  second  rotation  it  is  (0,  0,  .7071,  .7071).  Using  equation  (4.21)  to  multiply  these  two  quaternions,  we 
see  that  the  combined  rotation  is  represented  by  the  quaternion  (.5,  .5,  .5,  .5).  Figure  10.6,  below. 
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attitude  quaternion  components  vs.  time 
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Figure  10.6:  Quaternion  Elements  for  Successive  90  Degree  Rotations  as  Produced  by  Attitude_sim.m 

illustrates  that  these  results  are  obtained  from  attitude_sim.m.  The  first  rotation  takes  the  body  frame  from 
the  Euler  angles  \|/=0°,  0=0°,  <p=0°,  to  \j/=90°,  0=90°,  9=270°,.  The  second  takes  the  frame  to  a  final  attitude 
of  v|/=90°,  0=90°,  9=0°.  Figure  10.7  illustrates  the  successful  generation  of  these  angles  by  attitude_sim.m. 
This  set  of  rotations  is  a  good  test  of  the  simulation  software  since  it  encompasses  orientations  that  could 
cause  difficulty  due  to  the  singularities  inherent  in  the  Euler  angle  equations.  As  illustrated  here,  the 
expected  results  are  achieved  despite  this. 

10.1.4  Single  120  Degree  Rotation 

As  a  final  demonstration  of  the  ability  of  attitude_sim.m  to  correctly  simulate  desired  rotations,  this  final 
section  gives  the  results  for  a  single  rotation  about  a  vector  in  the  direction  of  (1,1,1)  with  a  magnitude  of 
120  degrees.  This  is  another  easily  visualized  rotation  that  allows  for  validation  of  correct  simulation.  The 
results  of  this  rotation  should  match  those  from  the  last  section  since  a  single  rotation  of  120  degrees  about 
(1,  1,  1)  is  equivalent  to  successive  90  degree  rotations  about  the  second  axis  and  then  about  the  third  axis. 
The  simulation  is  generated  with  a  constant  rotational  rate  of  0.19245  rev/min  about  each  body  axis  for  a 
duration  of  60  sec.  This  rate  is  determined  by  calculating  the  equal  rate  about  each  axis  that  will  produce  a 
rotation  of  1/3  revolution,  or  120  degrees.  Figure  10.8  below  gives  the  overall  view  of  the  motion.  Again, 
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3-D  plot  of  inertial  and  rotated  Sun  and  mag  field  vectors  and  rotation  quaternion 


Figure  10.8:  Single  120  Degree  Rotation  About  (1,1,1) 
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the  inertial  (1,  0,  0)  “Sun”  vector  has  coordinates  (0,  0,  1)  as  seen  from  the  rotated  frame.  The  inertial  (0, 
0,  1)  “mag  field”  vector  is  represented  as  (0,  1,  0)  in  the  rotated  frame.  These  results  do  indeed  match 
those  of  the  last  section,  and  those  that  can  be  visualized.  For  these  figures,  the  motion  is  again  simulated 
at  a  .01  sec  time  step,  but  only  every  100th  point  is  plotted  for  clarity.  The  next  figure  demonstrates  the 
quaternion  elements,  which  again  match  those  of  the  previous  section,  as  expected.  Using  equation  (4.13), 
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Figure  10.9:  Quaternion  Elements  for  Single  120  Degree  Rotation  About  (1,1,1) 

it  is  clear  that  (.5,  .5,  .5,  .5)  is  the  correct  quaternion  for  a  120  degree  rotation  about  an  axis  in  the  direction 
of  (1,  1,  1).  Finally,  Figure  10.10  depicts  the  Euler  angles  for  this  rotation.  The  set  \|/=90°,  0=90°,  <p=0° 
matches  the  expected  result. 

These  results  demonstrate  that  attitude_sim.m  generates  the  desired  results  given  a  rotational  rate  profile. 
In  each  case,  the  initial  Euler  angles  are  \p=0°,  0=0°,  <p=0°,  corresponding  to  the  body  frame  aligned  with 
the  inertial  frame.  The  rate  profile  then  causes  a  rotation  of  the  body  frame  to  a  final  orientation.  While 
these  three  cases  are  a  small  subset  of  those  simulated,  they  represent  easily  visualized  situations  where 
validation  of  the  results  is  facilitated.  At  the  same  time,  they  encompass  orientations  that  pose  potential 
difficulties  due  to  singularities.  In  each  case,  the  expected  results  are  faithfully  produced. 
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Figure  10.10:  Euler  Angles  for  Single  120  Degree  Rotation  About  (1,1,1) 

10.2  Computer  Simulation  of  Noisy  Sensor  Measurements 

In  addition  to  simulating  the  “true”  motion  of  the  object,  attitude_sim.m  simulates  noisy  measurements. 
This  section  discusses  how  this  is  accomplished. 

10.2.1  Corrupting  “Truth”  with  Noise  to  Simulate  Sensor  Measurements 

Attitude_sim.m  has  the  capability  to  simulate  noisy  Sun  sensor  measurements,  noisy  magnetic  field  sensor 
measurements,  and  noisy  rate  measurements.  For  the  two  vectors,  random  noise  having  a  user-defined 
standard  deviation  is  added  to  each  component  at  each  time  step.  This  noise  is  zero-mean  Gaussian  noise, 
and  a  different  random  value  is  added  to  each  component.  Similarly,  zero-mean  Gaussian  noise  is  added 
to  each  rotational  rate  at  each  time  step.  In  this  manner,  sensor  measurements  are  simulated  that  have 
random  noise  with  a  standard  deviation  equal  to  the  expected  sensor  accuracy.  While  real  world  noise  is 
not  often  truly  white,  zero-mean,  and  Gaussian,  it  is  often  simulated  this  way  for  a  number  of  reasons. 
First,  lacking  a  better  criterion  for  what  noise  to  expect,  Gaussian  noise  is  an  oft-made  assumption  that 
produces  good  results.  Second,  as  Zarchan  points  out,  while  “white  noise  is  not  physically  realizable,  it 
can  be  used  to  serve  as  an  invaluable  approximation  to  situations  in  which  a  disturbing  noise  is  wideband 
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compared  to  the  system  bandwidth.  In  addition,  white  noise  is  useful  for  analytical  operations  because  of 
the  impulsive  nature  of  the  autocorrelation  function  (it  makes  integrals  disappear)  [16]  ”  In  this  case, 
where  the  data  is  being  simulated  and  where  no  better  model  for  the  noise  exists,  Gaussian  noise  is 
assumed  for  the  reasons  stated  above.  The  software  file,  attitude_sim.m,  could  be  modified  relatively 
easily  for  future  simulations  where  a  different  noise  profile  is  desired. 

10.2.2  Simulating  a  Sensor  or  Measurement  Bias 

In  order  to  simulate  a  bias  on  one  or  more  sensors,  a  set  bias  may  be  added  to  each  array  value  generated 
by  attitude_sim.m.  While  this  capability  is  not  automated  in  the  current  code,  this  operation  is  easily 
accomplished  in  MATLAB.  In  this  manner,  a  separate  bias  may  be  added  to  each  component  of  each 
sensor,  providing  great  flexibility  in  evaluating  performance. 

10.2.3  Simulating  Data  ‘^Dropouts” 

Similar  to  the  situation  with  adding  bias  to  measurements,  a  drop-out  may  be  simulated  in  any,  or  all,  of 
the  sensor  measurements.  The  capability  to  simulate  dropouts  of  all  Sun  sensor  measurements  for  a 
period,  all  magnetometer  measurements  for  a  period,  or  all  rate  data  for  a  period  is  currently  built  into 
attitude_sim.m  as  a  user  selected  option.  If  implemented,  the  user  enters  which  sensor  to  drop  out,  the 
number  of  measurements  to  be  lost,  and  at  what  point  in  the  simulation  the  drop-out  should  begin.  These 
drop-outs  consist  of  the  sensor  data  being  set  to  zero  for  these  measurements.  In  this  fashion,  the  user  can 
analyze  the  effect  of  a  loss  of  signal  or  a  failed  or  intermittent  sensor. 

10.3  Measurement  of  Algorithm  Performance 

In  order  to  judge  how  well  the  algorithm  is  estimating  attitude  motion,  some  measure  of  performance  must 
be  defined.  For  this  effort,  measures  of  performance  are  based  on  mean-square-error  between  the  “true” 
values  and  those  estimated  by  the  algorithm.  This  section  details  how  the  measures  are  defined  for  each 
quantity  estimated. 

10.3.1  Attitude  Estimation  Performance 

The  attitude  estimate  from  the  algorithm  is  in  the  form  of  an  attitude  quaternion.  Since  the  true 
quaternion  is  available  from  attitude_sim.m,  a  straight  comparison  of  quaternion  components  could  be 
accomplished  to  judge  performance.  However,  since  the  quaternion  is  essentially  describing  a 
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transformation  between  the  body  frame  and  the  inertial  frame,  a  more  telling  measure  of  performance  is  a 
comparison  of  the  known  reference  vector  rotated  by  the  “true”  quaternion  and  that  rotated  by  the 
estimated  quaternion.  Therefore,  at  each  time  step,  the  reference  vector  composed  of  the  inertial  Sun 
vector  concatenated  with  the  inertial  magnetic  field  vector  is  rotated  using  the  “true”  attitude  quaternion 
for  that  time  step  provided  by  attitude_sim.m.  Also  at  each  time  step,  the  best  estimate  of  the  quaternion 
from  the  algorithm  is  used  to  rotate  the  inertial  reference  vector  to  the  body  frame.  Then  the  mean  square 
error  is  calculated  between  the  two  rotated  vectors.  This  value  is  used  to  measure  how  well  a  particular 
method  using  a  given  set  of  parameters  performs  in  terms  of  estimating  attitude  at  each  time  step. 

10.3.2  Rotational  Rate  Estimation  Performance 

The  algorithm  also  estimates  rotational  rates.  The  accuracy  of  this  estimate  must  also  be  assessed.  Again, 
a  mean  square  error  criterion  is  used.  Here  the  “true”  rotational  rates  at  each  time  step  are  known  from  the 
user  defined  rate  profile.  The  mean  square  error  of  the  estimated  rate  vector  is  determined  at  each  time 
step  by  comparing  the  rates  about  each  of  the  body  axes.  This  value  is  used  to  measure  how  well  the 
algorithm  is  estimating  the  overall  rate  vector  at  each  time  step. 

10.3.3  Overall  Figure  of  Merit 

Rather  than  attempt  to  scrutinize  the  attitude  and  rate  performance  at  each  time  step,  an  overall  figure  of 
merit  is  defined  to  judge  performance  over  the  length  of  an  entire  simulation  run.  This  aids  in  comparing 
the  results  from  various  algorithms  using  various  parameters  without  a  painstaking  time  step  by  time  step 
analysis.  First,  the  overall  attitude  estimation  performance  is  calculated  by  taking  an  average  of  the  mean 
square  error  over  all  the  time  steps  in  a  run.  Second,  the  overall  rate  estimation  performance  is  calculated 
by  taking  an  average  of  the  mean  square  error  over  all  the  time  steps  in  the  run.  Finally,  these  two  are 
combined  to  yield  the  overall  “figure  of  merit.”  The  two  are  combined  by  taking  the  square  root  of  the 
sum  of  the  squares  of  the  two  averages.  This  provides  a  single  number  to  judge  comparative  performance 
between  different  algorithms,  or  between  separate  filtering  runs  on  the  same  data,  by  the  same  algorithm, 
but  with  different  parameters.  Certainly,  the  attitude  or  rate  performance  can  still  be  evaluated 
independently  since  the  average  of  the  mean  square  error  is  available  for  each. 

10.4  Error  Minimization  via  Gauss-Newton 

The  software  to  implement  the  Gauss-Newton  error  minimization  routine  is  found  in  Appendix  A.  As 
developed  in  Chapter  9,  this  part  of  the  estimation  algorithm  has  a  number  of  parameters  that  must  be 
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defined.  From  a  programming  standpoint,  it  is  necessary  to  determine  what  the  convergence  criteria  will 
be  and  what  maximum  number  of  iterations  will  be  allowed.  As  part  of  this  effort,  a  study  of  the  number 
of  iterations  versus  error  performance  was  conducted.  As  reported  by  Marins  [11],  the  algorithm  nearly 
always  converges  in  very  few  steps,  often  in  one  or  two.  The  application  in  that  work  always  began  with  a 
first  guess  very  near  the  true  value.  In  this  application,  the  guess  will  be  relatively  close  to  the  true  value, 
but  due  to  the  nature  of  the  low-cost  sensors,  is  often  further  from  the  true  value  than  was  encountered  by 
Marins.  As  a  result,  it  is  important  to  evaluate  what  the  optimum  parameters  are  and  to  demonstrate  the 
convergence  properties  of  the  Gauss-Newton  routine. 

10.4.1  Convergence  Tolerance  and  the  Optimum  Number  of  Steps 

A  large  number  of  runs  were  conducted  using  the  gauss_newton.m  routine  to  examine  its  performance.  As 
developed  in  Chapter  9,  the  routine  determines  the  “best”  rotation  quaternion  that  minimizes  the  error 
between  the  known  true  reference  vector  and  the  measured  vector.  With  an  emphasis  on  minimizing  run 
time,  in  order  to  allow  real-time  implementation,  there  may  be  a  maximum  number  of  iterations  that 
should  be  allowed  as  the  routine  attempts  to  minimize  error.  In  other  words,  even  if  the  convergence 
criterion,  or  tolerance,  has  not  been  met,  the  routine  will  stop  iterating  and  produce  an  estimate  of  the 
rotation  quaternion.  In  this  implementation,  the  convergence  criteria,  or  tolerance,  is  the  mean  square 
error  found  using  the  e  error  vector  defined  earlier.  Since  the  sensors  provide  noisy  measurements,  in 
some  few  instances,  the  minimum  error  achieved  does  not  always  converge  to  a  value  that  meets  the 
convergence  criteria.  In  some  situations,  the  minimization  produces  a  steady-state  error  that  allows  for  a 
useful  estimated  quaternion  to  be  produced,  but  that  will  not  shrink  appreciably  no  matter  how  many 
iterations  are  allowed.  In  this  case,  in  order  to  minimize  run  time,  it  is  beneficial  to  terminate  the  loop  at 
some  small  number  of  iterations.  The  question  becomes  one  of  how  many  iterations  should  be  allowed. 
Since  the  routine  does  an  excellent  job  of  minimizing  the  error  in  a  small  number  of  steps,  this  maximum 
number  may  be  set  at  a  low  value.  This  holds  true  even  with  the  noisy  sensors  employed  here.  The 
following  table  shows  the  results  from  a  sample  of  simulation  runs  conducted  and  the  resulting  overall 
attitude  performance  for  various  combinations  of  convergence  tolerance  and  maximum  number  of 
iterations.  From  this  limited  excerpt  of  the  results,  it  may  be  seen  that  the  best  overall  performance,  in 
terms  of  minimum  error  while  minimizing  the  run  time,  is  achieved  using  a  tolerance  of  0.01  and  a 
maximum  number  of  iterations  set  to  10.  These  are  the  parameters  employed  in  the  Gauss-Newton  portion 
of  the  algorithm. 
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Table  10.1:  Gauss-Newton  Performance  for  Given  Convergence  Tolerance  and  Maximum  Iterations 


20  sec  run 

20  sec  run 

20  sec  run 

120  sec  run 

tolerance 

steps 

Figure  of  merit 

Figure  of  merit 

Figure  of  merit 

Figure  of  merit 

0.1 

5 

0.0185 

0.0231 

0.0241 

— 

0.1 

10 

0.0185 

0.0231 

0.0241 

0.0634 

0.1 

20 

0.0185 

0.0231 

0.0241 

0.0634 

0.1 

30 

0.0185 

0.0231 

— 

— 

0.01 

5 

0.0184 

0.0215 

0.0222 

0.0594 

0.01 

10 

0.0183 

0.0212 

0.0218 

0.0589 

0.01 

15 

— 

0.0213 

0.0218 

0.059 

0.01 

19 

— 

— 

— 

0.059 

0.01 

20 

0.0182 

0.0212 

0.0218 

0.0591 

0.01 

21 

0.0186 

— 

— 

0.059 

0.01 

22 

0.0182 

— 

— 

— 

0.01 

23 

0.0186 

— 

— 

— 

0.01 

24 

0.0182 

— 

— 

— 

0.01 

25 

0.0186 

— 

0.0221 

0.059 

0.01 

26 

0.0182 

— 

— 

— 

0.01 

27 

0.0186 

— 

— 

— 

0.01 

28 

0.0182 

— 

— 

— 

0.01 

29 

0.0186 

— 

— 

— 

0.01 

30 

0.0182 

0.0212 

0.0221 

0.0591 

0.01 

50 

0.0182 

0.0212 

— 

— 

0.01 

100 

0.0182 

— 

0.0218 

— 

0.05 

100 

— 

— 

0.0224 

— 

0.005 

100 

— 

— 

0.0218 

— 

0.001 

100 

— 

— 

0.0218 

— 

10.4.2  Convergence  of  the  Error  Minimization  Routine 


In  this  section,  two  sample  cases  from  the  same  successful  estimation  run  are  provided  to  illustrate  the 
typical  behavior  of  the  algorithm.  The  first  case,  illustrated  in  Figure  10.11,  demonstrates  the  most  often 
encountered  case  in  which  the  convergence  tolerance  is  met  in  only  one  or  two  steps.  Recall  that  the 
initial  guess  for  the  Gauss-Newton  routine  is  the  quaternion  (0,0,0, 1)  for  the  first  step,  and  the  previous 
best  estimate  for  all  subsequent  steps.  Here  it  is  clear  that  the  error  goes  to  some  very  small  value  after 
only  two  steps.  Many  times  this  convergence  will  occur  in  a  single  step.  Figure  10.12  illustrates  the 
corresponding  quaternion  elements  at  each  time  step  for  the  same  case.  In  fact,  this  is  the  case  for  a 
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mean  square  error  at  each  iteration  step 


Figure  10.1 1:  Single  Time  Step  Error  History  for  Successful  Convergence 


attitude  quaternion  components  versus  iteration  step 


Figure  10.12:  Single  Time  Step  Quaternion  Component  History  for  Successful  Convergence 
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rotation  of  120  degrees  about  an  axis  of  (1,  1,  1).  In  Section  10.1.4  it  was  demonstrated  that  this  rotation 
corresponds  to  a  rotation  quaternion  of  (.5,  .5,  .5,  .5).  Here  we  see  that,  in  this  successful  convergence 
case,  the  estimated  quaternion  is  close  to  (.5,  .5,  .5,  .5),  but  not  exact.  This  is  due  to  the  error  in  the  sensor 
measurements.  While  it  at  first  may  not  appear  very  close,  this  combination  of  quaternion  elements  meets 
the  mean  square  error  convergence  criteria,  and  is  considered  a  “converged”  case.  The  next  two  figures 
demonstrate  the  case  where,  as  described  above,  the  convergence  criterion  is  not  successfully  met.  These 
two  figures  are  from  the  same  run  as  the  two  previous  figures,  but  earlier,  before  the  rotation  had 
approached  120  degrees.  It  is  clear  from  these  figures  that  increasing  the  number  of  iterations  will  not 
result  in  a  significant  reduction  in  error,  since  the  mean  square  error  has  no  slope  and  will  not  decrease 
further.  This  case  demonstrates  the  utility  of  setting  a  relatively  small  maximum  for  the  number  of 
iterations  allowed,  thereby  reducing  run  time  of  the  routine.  In  fact,  the  majority  of  the  error  minimization 
occurs  during  the  first  few  iterations,  and  even  though  the  MSE  convergence  criterion  is  not  met,  a  useful 
estimate  of  the  attitude  quaternion  is  produced  and  is  available  for  input  to  the  filter  algorithm. 


mean  square  error  at  each  iteration  step 


Figure  10.13:  Single  Time  Step  Error  History  for  Unsuccessful  Convergence 
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attitude  quaternion  components  versus  iteration  step 


Figure  10.14:  Single  Time  Step  Quaternion  Component  History  for  Unsuccessful  Convergence 

10.5  Demonstrated  Performance  of  the  Extended  Kalman  Filter 

Every  filtering  algorithm  has  a  number  of  parameters  that  must  be  set  to  appropriate  values  for  optimum 
operation  of  the  filter.  This  process  of  determining  the  appropriate  values  and  setting  them  in  the 
algorithm  is  often  referred  to  as  “tuning”  the  filter.  As  described  in  Chapters  8  and  9,  the  EKF  has  several 
such  values  that  must  be  determined.  This  section  will  discuss  in  more  detail  how  these  values  are  arrived 
at,  and  what  values  are  used  for  the  EKF  designed  in  Chapter  9.  Later  in  this  section,  the  proper  operation 
of  the  EKF  will  be  demonstrated  via  examples  illustrating  performance  both  with  and  without  the 
inclusion  of  sensors  that  measure  rotational  rates. 

10.5.1  “Tuning”  the  Extended  Kalman  Filter 

The  extended  Kalman  filter  designed  in  Chapter  9  has  a  number  of  parameters  whose  value  must  be 
determined  for  optimum  operation  of  the  filter  in  order  to  achieve  the  lowest  possible  error  in  the  state 
estimate.  As  noted  earlier,  formulating  the  Measurement  Noise  Matrix,  R ,  depends  upon  an  accurate 
representation  of  v ,  the  vector  containing  the  measurement  standard  deviations.  The  values  for  these 
were  addressed  in  earlier  sections,  and  these  standard  deviations  are  user-selectable  in  the  software  routine 
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attitude_sim.m.  In  addition  to  these  noise  figures  which  may  be  ascertained  through  testing, 
specifications,  etc.,  there  are  time  constant  parameters  embedded  within  the  process  model  illustrated  in 
Figure  9.3.,  and  there  is  process  noise  that  goes  into  the  calculation  of  the  Process  Noise  Matrix,  Qy  as 
indicated  in  equations  (9.36)  and  (9.38).  As  mentioned  earlier,  one  of  the  drawbacks  of  using  “optimal” 
filters  is  the  somewhat  complex  design  and  tuning  process.  Several  authors  have  made  reference  to  the 
fact  that  “the  approach  is  generally  one  of  analysis  rather  than  synthesis,  and  a  number  of  steps  are 
necessary  before  a  satisfactory  result  emerges  [71],”  and  that  “Kalman  filter  design  is  both  science  and  art, 
and  different  applications  and  operating  environments  call  for  grossly  different  Kalman  filters  [86].”  The 
next  few  sections  will  attempt  to  shed  some  light  on  how  this  process  plays  out  for  this  filter  and 
application. 

10.5.1.1  Choosing  an  Integration  Scheme 

Embedded  within  the  EKF  structure  is  a  routine  for  the  numerical  integration  of  the  nonlinear  dynamic 
equations.  Two  choices  must  be  made  with  respect  to  this  integration.  First,  the  quickest  acceptable 
method  is  desired  so  as  to  minimize  run  time  in  an  effort  to  yield  an  algorithm  that  can  be  implemented  in 
real  time.  “Acceptable”  is  defined,  here,  as  having  sufficient  accuracy  such  that  errors  due  to  numerical 
integration  do  not  become  significant.  Second,  the  optimum  step  size  must  be  determined,  again  balancing 
the  tradeoff  between  algorithm  speed  and  accuracy.  As  noted  in  [17],  many  numerical  integration 
techniques  exist  for  integrating  differential  equations.  Through  many  examples,  Zarchan  demonstrates 
that  a  second-order  Runge-Kutta  technique  works  well,  producing  accurate  answers  without  undue 
computational  load.  This  method  is  used  almost  exclusively  within  [17]  for  the  propagation  of  states  due 
to  its  simplicity,  ease  of  programming  and  because  of  the  author’s  extensive  successful  experience  with  it. 
In  at  least  one  example,  however,  Zarchan  points  out  that  similar  results  are  sometimes  achieved  using  the 
simpler  Euler  integration  method  with  the  addition  of  process  noise  to  account  for  the  less  accurate 
propagation  [17],  In  this  work,  extensive  simulation  with  both  a  single  step  Euler  integration  and  the 
second-order  Runge-Kutta  routines  was  conducted,  and  the  more  complex  routine  does  not  improve 
performance.  In  fact,  due  to  the  highly  simplified  model,  the  Euler  method  is  the  better  performer,  at  all 
time  steps.  Therefore,  in  order  to  decrease  the  computation  time  due  to  numerical  integration,  the  single 
step  Euler  method  was  implemented.  The  second  question  posed  above  is  what  step  size  to  use.  This 
requires  some  experimentation,  because  again  there  is  a  desire  to  use  the  largest  “acceptable”  step  size  in 
order  to  minimize  run  time.  Zarchan  describes  one  approach  that  is  somewhat  more  structured  than 
simply  guessing  a  time  step: 

The  integration  step  size  h  must  be  small  enough  to  yield  answers  of  sufficient  accuracy. 

A  simple  test,  commonly  practiced  among  engineers,  is  to  find  the  appropriate  integration 
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step  size  by  experiment.  As  a  rule  of  thumb  the  initial  step  size  is  chosen  to  be  several 
times  smaller  than  the  smallest  time  constant  in  the  system  under  consideration.  The  step 
size  is  then  halved  to  see  if  the  answers  change  significantly.  If  the  new  answers  are 
approximately  the  same,  the  larger  integration  step  size  is  used  to  avoid  excessive 
computer  running  time.  If  the  answers  change  substantially,  then  the  integration  interval 
is  again  halved  and  the  process  is  repeated  [16]. 

Following  a  similar  approach,  the  following  sample  results  were  generated  experimentally  for  one  of  the 
EKF  routines.  The  numbers  in  the  body  of  the  table  are  the  average  mean  square  error  for  the  rotated 
reference  vector  and  the  overall  figure  of  merit,  as  described  earlier,  for  the  run.  The  lower  the  figure  of 
merit  is,  the  better  the  estimation  of  attitude  and  rates. 

Table  10.2:  Sample  Experimental  Data  for  Determining  Optimum  EKF  Integration  Step  Size 


Integration  Step  Size 

Rotated  Vector  Average  MSE 

Overall  Figure  of  Merit 

0. 1  sec 

.0110 

.0143 

0.01  sec 

9.72e-4 

.0011 

0.001  sec 

9.72e-4 

.0011 

0.0001  sec 

9.72e-4 

.0011 

This  is  one  example  of  the  simulation  runs  accomplished  to  determine  the  best  step  size  for  the  numerical 
integration  of  the  nonlinear  dynamic  equations  used  to  propagate  the  system  states  forward.  These  results 
are  somewhat  unexpected.  Typically,  a  shorter  time  step  will  produce  a  better  result  when  numerically 
integrating  a  function.  What  is  demonstrated  here,  however,  is  that  further  decreasing  the  time  step  does 
not  improve  overall  performance.  This  may  be  attributed  to  the  very  rudimentary  model  embedded  within 
the  filter  from  which  the  dynamic  equations  are  generated.  Its  sole  purpose  is  to  propagate  the  states 
forward  one  time  step,  within  the  model,  and  the  key  criteria  is  that  the  time  step  is  short  relative  to  the 
time  basis  of  the  motion.  As  noted  earlier,  for  the  cases  simulated  here,  the  highest  frequency  motion  is 
the  225  rev/min  motion  about  the  longitudinal  axis  of  the  rocket,  and  this  corresponds  to  a  time  scale  of 
approximately  .013  sec.  Based  on  the  performance  measures  seen  in  the  table,  it  is  clear  that  decreasing 
the  step  size  beyond  0.01  seconds  does  not  improve  performance.  This  assumes,  of  course,  that  the  time 
interval  at  which  measurements  are  available  is  not  less  than  0.01  seconds.  In  cases  where  the  interval  is 
smaller,  a  step  size  of  shorter  duration  must  be  used  for  optimum  results.  For  the  cases  simulated  here,  to 
minimize  run  time,  0.01  seconds  is  used  in  the  Euler  integration  routine. 
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10.5,1.2  Setting  Process  Noise,  Os 

Process  noise  in  a  Kalman  type  filter  is  a  reflection  of  the  uncertainty  in  the  process  model  implemented 
by  the  filter.  This  can  be  very  difficult  to  ascertain  analytically,  and  even  in  cases  where  the  model  is 
known  to  be  perfect,  there  can  be  reasons  to  artificially  inflate  the  process  noise.  In  cases  where  the 
system  dynamics  are  known  to  be  imperfectly  modeled,  it  is  often  “advantageous  to  over-estimate  the 
system  noise  so  that  Qk  is  larger  than  strictly  necessary  [34].”  As  noted  previously,  the  process  noise  is 
used  to  initialize  the  Process  Noise  Matrix  and  appears  in  the  computation  of  the  Discrete  Process  Noise 
Matrix  at  each  iteration  of  the  filter.  Adding  fictitious  process  noise  to  the  actual  estimate  of  the 
uncertainty  in  the  model  is  a  commonly  accepted  engineering  solution  for  many  of  the  problems 
encountered  when  implementing  EKFs.  One  such  example  is  when  divergence  is  encountered  in  the  error 
in  the  estimate.  One  cause  of  such  divergence  is  inaccurately  propagating  the  states  forward  in  time.  The 
obvious  solution  is  to  choose  a  more  accurate  numerical  integration  technique,  or  a  smaller  time  step. 
Either  approach  results  in  greater  computational  load.  Another  solution,  often  employed,  is  the  addition  of 
process  noise  to  the  filter  [17],  [75].  In  many  cases,  this  will  prevent  the  divergence  while  allowing  the 
use  of  a  less  computationally  intense  integration  method  and  step  size.  The  price  to  be  paid,  however,  is 
typically  a  noisier  estimate  of  the  state  than  would  be  achieved  with  a  more  accurate  propagation  [17].  Put 
another  way,  “increasing  the  process  noise  effectively  increases  the  bandwidth  of  the  filter,  which 
improves  its  tracking  capabilities  at  the  expense  of  more  noise  transmission  [17].”  From  this  perspective, 
increasing  the  process  noise,  even  artificially,  increases  the  bandwidth  of  the  filter  and  makes  the  filter  less 
sluggish,  while  a  lower  process  noise  might  cause  the  filter  to  lag  the  actual  signal  [17].  In  this  way,  an 
EKF  is  like  any  other  filter,  higher  bandwidth  often  means  quicker  response,  but  also  allows  in  more  noise. 
As  a  practical  matter,  setting  the  process  noise  too  low  “eventually  causes  the  filter  to  stop  paying 
attention  to  new  measurements  (i.e.,  filter  bandwidth  is  reduced  or  the  filter  gain  is  too  small  for 
incorporating  more  measurements).  In  other  words,  a  zero  process  noise  extended  Kalman  filter 
eventually  goes  to  sleep  [17]!” 

For  this  application,  the  process  model  is  known  to  be  a  simplified  version  of  the  real  processes  producing 
rotational  motion  of  the  rocket.  This  simplification  is  intentional  both  because  the  true  model  is  very 
complex,  and  so  that  one  model  is  easily  transportable  between  various  vehicles.  As  a  result,  the  process 
noise  is  known  to  be  non-zero.  As  is  usually  the  case,  for  the  reasons  noted  above,  the  analytical 
determination  of  how  much  process  noise  to  add  is  very  difficult.  As  an  alternative,  the  optimum  process 
noise  for  the  algorithms  developed  here  is  determined  empirically.  Admittedly,  this  is  a  luxury  of  working 
with  simulated  data,  where  the  “truth”  is  known  and  where  the  same  data  may  be  processed  repeatedly  to 
determine  the  value  of  process  noise  that  produces  the  best  performance.  Each  algorithm  allows  the  user 
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to  enter  the  amount  of  process  noise  to  be  used  for  that  filter  run,  therefore  this  can  be  easily  varied  to 
evaluate  the  results  corresponding  to  different  amounts  of  process  noise  while  holding  all  else  equal. 

This  procedure  yields  the  results  in  the  table  below  for  a  baseline  example  consisting  of  a  profile  with 
rotational  rates  that  asymptotically  approach  final  values  of  0.5  rev/min  about  the  body  x~axis,  0.5  rev/min 
about  the  body  y-axis,  and  225  rev/min  about  the  body  z-axis.  In  each  case  the  profile  achieves  its  final 
value  at  20  seconds  into  the  run  with  a  total  simulation  time  of  40  sec.  This  simulation  used  the  baseline 
sensor  standard  deviations  described  in  Chapter  6  of  1.333  degrees  in  each  axis  for  the  Sun  sensor,  3.333 
degrees  in  each  axis  for  the  magnetometer,  and  0.333  rev/min  for  the  rate  sensors.  Following  this 
procedure,  the  following  data  was  generated  for  the  EKF  using  measured  rates. 

Table  10.3:  Sample  Experimental  Data  for  Determining  Optimum  Process  Noise  for  EKF 


II 

*5 

To>i 

EKF  Average 

MSE  of  Rotated 

Reference  Vector 

EKF  Figure  of 

Merit 

36 

36 

le7 

.0010 

.0012 

38 

36 

le7 

.0010 

.0011 

• 

• 

: 

; 

• 

112 

36 

le7 

9.72e-4 

.0011 

: 

• 

: 

• 

185 

36 

le7 

9.68e-4 

.0011 

190 

36 

le7 

9.68e-4 

.0012 

This  is  a  small  subset  of  the  data  generated  for  one  run  using  one  algorithm,  the  EKF  with  rate  sensor  data. 
Values  outside  this  range  were  tested,  all  with  higher  figures  of  merit.  This  is  meant  to  show  a  “close-up” 
of  the  range  of  values  producing  the  optimum  results.  An  example  of  a  complete  set  is  found  in  the 
appendices.  Here  it  is  evident  that  the  “tuning”  process  is  not  exact.  In  fact,  process  noise  values  of  38  - 
185  yield  almost  identical  results.  While  this  makes  the  EKF  “robust”  with  respect  to  different  values  of 
entered  process  noise,  it  also  makes  it  difficult  to  pin  down  an  optimum  number.  For  a  broad  range  of 
values  that  yield  similar  performance,  such  as  those  found  here,  a  median  value  is  typically  selected  as  the 
optimum.  For  this  particular  case,  the  value  selected  is  112.  By  comparison,  the  range  of  values  found 
when  no  rate  measurements  are  used  is  3.42  -  3.64,  and  an  “optimized”  value  of  3.53  is  used. 
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10.5,1.3  Setting  Motion  Time  Constants,  xr 

As  with  the  process  noise  discussed  in  the  previous  section,  the  motion  time  constants  from  the  process 
model  may  be  entered  by  the  user  when  running  the  software  that  implements  the  various  algorithms 

developed  here.  This  allows  the  time  constants  ,  and  f ^  to  be  varied  one  at  a  time  while 

holding  all  other  parameters  equal  to  evaluate  their  impact  on  the  performance  of  the  algorithm,  and  to 
determine  optimum  values  for  a  particular  set  of  data.  In  the  case  where  actual  data  is  being  post- 
processed,  these  constants  may  be  determined  using  an  automated  routine,  such  as  that  employed  in  [11], 
where  the  process  model  is  simulated  in  SIMULINK,  a  MATLAB  utility,  and  the  time  constants  and  noise 
variances  are  adjusted  until  the  best  match  with  the  real  data  is  achieved.  A  similar  approach  is  used  here 
for  the  simulated  data,  although  a  manual  iteration  of  runs  was  accomplished.  In  this  case,  the  optimum 
process  noise  is  determined  using  the  approach  described  in  the  last  section.  Then,  using  this  optimum 
process  noise,  the  time  constants  are  varied  until  the  set  producing  the  overall  minimum  figure  of  merit  is 
discovered.  Since  the  x-axis  and  y-axis  rate  profiles  in  the  baseline  trajectory  are  identical,  the  time 
constants  for  these  two  are  set  equal.  An  example  of  this  process,  for  the  baseline  trajectory  described  in 
the  last  section,  but  now  with  no  rate  measurements  used,  is  illustrated  by  the  sample  data  in  the  following 
table. 


Table  10.4:  Sample  Experimental  Data  for  Determining  Optimum  EKF  Time  Constants 


II 

*5 

EKF  Average 

MSE  of  Rotated 

Reference  Vector 

EKF  Figure  of 

Merit 

3.53 

23 

le7 

.0010 

.0689 

3.53 

24 

le7 

.0010 

.0688 

; 

• 

; 

; 

; 

3.53 

29 

le7 

.0010 

.0688 

• 

: 

: 

3.53 

33 

le7 

.0010 

.0688 

3.53 

34 

le7 

.0010 

.0689 

Again,  this  is  a  small  subset  intended  to  show  a  close-up  of  the  range  of  interest,  and  a  complete  example 
of  the  data  generated  is  found  in  the  appendices.  Note  that  the  optimized  <I>S  in  this  case  is  different  than 
for  the  example  in  Section  10.5.1.2,  because  this  is  for  a  case  where  differenced  rates  are  used  and  the 
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earlier  example  came  from  a  case  where  rate  measurements  are  used.  From  this  excerpt,  the  optimized 
value  for  t ^  is  29,  again  choosing  a  median  value  from  a  range  yielding  similar  performance  as 

determined  by  overall  figure  of  merit. 


For  all  the  runs  accomplished  in  this  work,  the  time  constants,  and  the  process  noise,  are  optimized 
empirically  to  provide  the  best  possible  figure  of  merit  to  allow  for  the  side-by-side  comparison  of  relative 
performance  between  algorithms.  While  these  tuning  parameters  are  undoubtedly  functions  of  the 
dynamics  of  the  motion,  they  are  also  clearly  functions  of  sensor  measurement  quality  and  the  effect  of  the 
suboptimal  dynamic  model  embedded  within  the  filter.  This  is  made  evident  by  the  fact  that  the  optimum 
time  constants,  and  process  noise,  are  different  for  each  of  the  four  algorithms  tested,  even  when 
processing  identical  data.  No  analytical  relationship  was  developed  for  determining  these  tuning 
parameters.  In  every  case  they  were  optimized  through  the  iterative  process  described  in  this  section  and, 
for  the  process  noise,  in  the  previous  section.  Clearly,  such  an  analytical  expression  would  be  a  very 
useful  tool  from  an  implementation  standpoint,  and  warrants  further  investigation. 

10.5.2  Sample  Performance  When  no  Rate  Measurements  are  Available 


As  described  in  Chapter  9,  the  EKF  has  been  implemented  to  work  with  and  without  actual  rate  sensor 
measurements.  This  section  describes  the  performance  of  the  filter  when  these  rate  measurements  are  not 
available  and  this  filter  is  implemented  in  norates_ekf.m  found  in  Appendix  A,  Of  interest  from  an 
implementation  standpoint,  this  method  requires  the  differencing  of  subsequent  quaternions  to  generate  a 
quaternion  rate.  This  in  turn  is  used  to  calculate  corresponding  body  rates  as  developed  in  equations  (9.2) 
and  (9.3).  Following  this  approach  obviously  incurs  a  one  time  step  delay  before  estimates  may  be 
generated.  For  small  time  steps,  however,  this  should  not  be  a  major  impact  although  it  must  be  evaluated 
for  any  projected  application.  If  such  a  delay  is  acceptable,  as  it  certainly  would  be  in  a  post-processed 
application,  then  a  minimum  sensor  suite  composed  only  of  two  vector  sensors  may  be  employed,  without 
the  need  for  rate  sensors.  The  following  table  illustrates  the  type  of  performance  available  from  this 
implementation.  This  data  was  generated  for  the  duration  of  simulation  shown  using  sensor  measurement 
standard  deviations  of  1.333  degrees  for  each  axis  of  the  Sun  sensor  and  3.333  degrees  for  the 
magnetometer  measurements.  The  rate  profiles  asymptotically  approach  the  final  values  of  0.5  rev/min, 
0.5  rev/min,  and  225  rev/min  about  the  body  x,  y,  and  z  axes  respectively.  The  final  values  are  achieved  at 

20  seconds.  The  integration  step  size  is  the  standard  0.01  seconds.  Where,  again,  is  the  process  noise, 
T  =  %  is  the  time  constant  associated  with  ©i  and  ©2  and  f  is  the  time  constant  associated  with  ©3. 

tU||  U>> 

The  MSE  and  the  figure  of  merit  are  as  defined  earlier.  Table  10.6  displays  a  number  of  “angle” 
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Table  10.5:  Sample  EKF  Performance  When  no  Rate  Measurements  are  Available 


Duration 

II 

EKF  Average 

MSE  of 

Rotated 

Reference 

Vector 

EKF  Figure  of 

Merit 

40 

3.53 

29 

le7 

.0010 

.0688 

40 

3.57 

28 

le7 

9.71e-4 

.0679 

60 

3.10 

42 

le7 

9.94e-4 

.0571 

60 

3.09 

42 

le7 

9.78e-4 

.0587 

Table  10.6:  EKF  Angle  Measure  Performance  When  no  Rate  Measurements  are  Available 


MeanAngici 

^Anglel 

MaXAnglel 

MeanAngle2 

^Angle2 

MaXAngle2 

Mean  Angies 

0*Angle3 

MaXAngle3 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

2.75 

1.68 

12.69 

2.76 

1.73 

12.30 

3.23 

1.92 

13.61 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

2.80 

1.68 

11.44 

2.78 

1.69 

13.21 

3.27 

1.89 

12.21 

performance  measures  that  correspond  to  these  numerical  measures  of  performance.  While  the  figure  of 
merit  defined  earlier  is  the  adopted  standard  of  performance  for  relating  one  method  to  another  for  a  given 
set  of  data,  these  angle  measures  help  give  an  intuitive  feel  for  how  well  the  filter  is  performing.  Angle  1 
refers  to  the  angle  between  the  body  x  axis  rotated  using  the  “true”  quaternion  and  the  body  x-axis  rotated 
using  the  filter  estimated  quaternion,  or  the  “error”  angle  for  the  first  axis.  Angle2  and  Angle3  are 
similarly  calculated  for  the  body  y  and  z  axes  respectively,  a  indicates  the  standard  deviation  of  the  error 
angle.  The  rows  in  this  table  correspond  to  the  rows  in  the  previous  table,  coming  from  the  same 
simulated  data.  Any  table  entries  of  “N/A”  indicate  that  those  runs  were  accomplished  during  the  initial 
“tuning”  process,  using  the  figure  of  merit,  and  the  option  to  calculate  angles  was  not  selected. 

10.5.3  Sample  Performance  When  Rate  Measurements  are  Available 

The  EKF  has  also  been  implemented  to  accept  actual  noisy  rate  measurements  from  rate  sensors.  This 
implementation  is  accomplished  in  rates_ekf.m  found  in  Appendix  A.  This  section  describes  the 
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performance  of  the  filter  when  these  rate  measurements  are  available.  As  noted  in  [13],  and  demonstrated 
here,  the  use  of  rate  sensor  data  eliminates  much  of  the  “lag”  experienced  when  estimating  the  rotational 
rates  found  in  the  filter  that  does  not  incorporate  rate  measurements.  Use  of  these  sensors  also  eliminates 
the  one  time  step  delay  before  estimates  become  available.  These  two  improvements  come  at  the  expense 
of  greater  cost  and  complexity  to  include  rate  sensors  to  measure  rates  about  each  of  the  body  axes.  The 
following  table  illustrates  the  type  of  performance  available  from  this  implementation.  This  data  was 
generated  for  the  duration  of  simulation  shown  using  sensor  measurement  standard  deviations  of  1.333 
degrees  for  each  axis  of  the  Sun  sensor  and  3.333  degrees  for  the  magnetometer  measurements.  The  rate 
profiles  asymptotically  approach  the  final  values  of  0.5  rev/min,  0.5  rev/min,  and  225  rev/min  about  the 
body  x,  y,  and  z  axes  respectively.  The  final  values  are  achieved  at  20  seconds.  The  integration  step  size 
is  the  standard  0.01  seconds. 

Table  10.7:  Sample  EKF  Performance  When  Rate  Measurements  are  Available 


Duration  of 

Simulation 

(seconds) 

II 

*5 

EKF  Average 

MSE  of 

Rotated 

Reference 

Vector 

EKF  Figure  of 

Merit 

40  (set  #1) 

112 

36 

le7 

9.72e-4 

.0011 

40  (set  #2) 

122 

36 

le7 

9.49e-4 

.0011 

60  (set  #3) 

175 

36 

le7 

9.77e-4 

.0011 

60  (set  #4) 

175 

36 

le7 

9.62e-4 

.0011 

10.6  Demonstrated  Performance  of  the  Unscented  Kalman  Filter 

As  is  the  case  for  the  extended  Kalman  filter,  the  unscented  Kalman  filter  has  a  number  of  parameters  that 
must  be  set  to  appropriate  values  for  optimum  operation  of  the  filter.  This  section  will  discuss  how  these 
values  are  arrived  at,  and  what  values  are  used  for  the  UKF  that  was  designed  in  Chapter  9.  Later  in  this 
section,  the  proper  operation  of  the  UKF  will  be  demonstrated  via  examples  illustrating  performance  both 
with  and  without  the  inclusion  of  sensors  that  measure  rotational  rates. 
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Table  10.8:  EKF  Angle  Measure  Performance  When  Rate  Measurements  are  Available 


Mean  Angie  i 

Angle  1 

Max  Angle! 

MeanAngie2 

0AngIe2 

MaXAng!e2 

Mean  Angles 

^Ang!e3 

MaXAngie3 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

2.72 

1.65 

12.61 

2.73 

1.71 

12.26 

3.19 

1.91 

13.59 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

n 

2.77 

1.67 

11.47 

2.74 

1.68 

13.12 

3.24 

1.88 

^3 

10.6.1  “Tuning”  the  Unscented  Kalman  Filter 

The  unscented  Kalman  filter  incorporates  all  the  same  parameters  as  the  EKF,  but  also  has  several 
additional  values  that  must  be  determined  and  set  in  order  to  achieve  the  lowest  possible  error  in  the  state 
estimate.  As  for  the  EKF,  formulating  the  Measurement  Noise  Matrix,  /?,  in  the  UKF  depends  upon  an 
accurate  representation  of  v  ,  the  vector  containing  the  measurement  standard  deviations.  These  values 
were  addressed  in  earlier  sections,  and  these  standard  deviations  are  user-selectable  in  the  software  routine 
attitude_sim.m.  In  addition  to  these  noise  figures  which  may  be  ascertained  through  testing, 
specifications,  etc.,  there  are  time  constant  parameters  corresponding  to  each  of  the  rotational  rates 
embedded  within  the  process  model  illustrated  in  Figure  9.3.,  and  there  is  process  noise  that  goes  into  the 
calculation  of  the  Process  Noise  Matrix,  Q ,  as  indicated  in  equations  (9.51)  and  (9.52).  As  for  the  EKF, 
the  process  for  optimizing  these  values  for  the  UKF  is  not  an  easy  one.  The  next  few  sections  will  detail 
their  determination. 

10.6.1.1  Choosing  an  Integration  Scheme 

As  was  the  case  with  the  EKF,  the  UKF  employs  numerical  integration  to  propagate  states  forward  in 
time.  In  this  case,  however,  the  entire  augmented  set  of  “sigma”  points  described  in  Section  9.8.2.9  must 
be  propagated  through  the  nonlinear  system  dynamic  equations.  Since  there  are  2(2L)+ 1  points  in  the 
augmented  set,  or  a  total  of  29  in  this  case,  choosing  an  efficient  numerical  technique  is  very  important  to 
minimize  run  time.  As  was  done  with  the  EKF,  both  a  2nd  order  Runge-Kutta  and  a  one-step  Euler  method 
were  evaluated.  The  Euler  method  is  the  better  performer  in  addition  to  requiring  fewer  computational 
steps.  Because  the  same  motion  is  simulated  for  the  UKF  and  an  identical  suboptimal  dynamic  model  is 
used  to  generate  the  state  equations,  the  discussion  in  Section  10.5.1.1  applies  here  as  well.  Extensive 
simulation  with  different  time  steps  was  accomplished  and  an  optimum  time  step  of  .01  sec  was 
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determined,  for  the  cases  simulated  here.  The  following  table  displays  one  such  set  of  results  for  the  Euler 
method.  From  the  tabulated  data,  it  is  clear  that  the  .01  sec  step  size  achieves  the  desired  result  while 
minimizing  the  number  of  calculations. 


Table  10.9:  Sample  Experimental  Data  for  Determining  Optimum  UKF  Integration  Step  Size 


Integration  Step  Size 

Rotated  Vector  Average  MSE 

Overall  Figure  of  Merit 

0.1  sec 

.0831 

.0831 

0.01  sec 

4.48e-4 

6.37e-4 

0.001  sec 

4.48e-4 

6.37e-4 

0.0001  sec 

4.48e-4 

6.37e-4 

10.6.1.2  Setting  Process  Noise,  <&s 

Setting  process  noise  for  the  UKF  is  subject  to  the  same  guidelines  discussed  in  Section  10.5  for  the  EKF. 
Here  again,  additional  process  noise  is  often  used  to  cover  for  uncertainties  in  the  system  model,  noise 
parameters,  etc.  An  analytical  determination  is  intractable  in  this  case,  so  as  was  done  for  the  EKF,  the 
optimum  noise  is  determined  through  an  iterative,  empirical  process.  The  process  noise  is  varied  in  small 
increments  and  the  overall  performance  of  the  filter  is  evaluated  at  each  value.  The  minimum  “figure  of 
merit”  corresponds  to  the  optimum  value  for  the  process  noise.  The  following  is  an  excerpt  of  one  such 
application  of  the  process  for  the  baseline  trajectory. 

Table  10.10:  Sample  Experimental  Data  for  Determining  Optimum  Process  Noise  for  UKF 


*s 

II 

*5 

UKF  Average 

MSE  of  Rotated 

Reference  Vector 

UKF  Figure  of 

Merit 

1.09 

.07 

le7 

.0010 

.0450 

1.08 

.07 

le7 

.0010 

.0449 

1.07 

.07 

le7 

.0010 

.0449 

1.06 

.07 

le7 

.0010 

.0449 

1.05 

.07 

lei 

.0010 

.0449 

1.04 

.07 

lei 

1 

.0010 

.0450 
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For  this  particular  example  case,  it  is  evident  that  values  of  process  noise  between  1 .04  and  1 .09  yield  the 
best  overall  performance.  A  broader  range  of  values  was  tested,  and  this  table  shows  a  “close-up”  of  the 
range  of  interest.  Given  these  results,  a  value  of  1.07  is  used  as  the  optimized  process  noise.  This  is  one 
example  of  determining  the  process  noise.  Making  the  choice  of  this  parameter  more  difficult,  in  a  general 
sense,  is  the  fact  that  the  “optimum”  value  varies  with  changes  in  sensor  accuracy,  body  rate  profile,  and  is 
affected  by  whether  the  UKF  uses  measured  rates  or  rates  from  differencing.  This  parameter  must  be 
“tuned”  for  the  best  estimate  of  the  application  motion  characteristics.  As  noted  in  section  10.5,  no 
analytical  expression  was  determined  and  this  is  an  area  that  warrants  further  research. 

10.6.1.3  Setting  the  Time  Constants  of  Motion,  xr 

A  method  similar  to  that  for  determining  the  process  noise  is  employed  for  the  time  constants  embedded  in 
the  process  model.  Again,  in  the  case  where  actual  rotational  data  is  available,  the  variances  and  time 
constants  may  be  determined  through  post-processing  using  an  automated,  iterative  procedure  as  that  used 
by  Marins  [11]  and  described  in  Section  10.5.  Here,  a  manual  iterative  process  is  used  where  one  time 
constant  is  varied  while  all  other  parameters  are  held  constant.  Once  a  minimum  figure  of  merit  is 
achieved,  the  optimized  time  constant  is  determined.  Since  the  o>i  and  ©2  profiles  are  identical,  the  time 
constants  associated  with  these  are  set  equal  to  each  other.  This  is  a  luxury  of  using  simulated  data,  where 
the  same  data  may  be  processed  again  and  again  while  varying  one  parameter  at  a  time.  A  similar 
approach  could  be  used  with  real  data  if  the  flight  was  post-processed,  but  for  a  real-time  approach  a  best 
estimate  would  have  to  be  used  or  other  measures  taken  to  predict  the  optimum  time  constants  associated 
with  the  motion.  An  excerpt  of  the  data  generated  for  the  baseline  trajectory  is  shown  in  Table  10.1 1.  For 
this  particular  example,  choosing  a  median  value  from  the  range  producing  similar  results  gives  an 

optimized  time  constant  for  6^  and  co^  of  0.07.  A  similar  process  would  then  be  completed  for 
determining  . 

10.6.1.4  Choosing  the  UT  Parameters 

Thus  far,  tuning  the  UKF  has  been  identical  to  tuning  the  EKF,  although  different  optimum  parameter 
values  are  found  for  each  when  processing  the  same  data.  In  addition  to  the  process  noise,  and  the  three 
time  constants,  there  are  several  other  parameters  that  differ  from  those  found  in  the  EKF.  These  were 
identified  in  Chapter  9  in  equations  (9.56)  and  (9.61)  for  the  original  set  of  sigma  points,  and  in  (9.63)  and 
(9.64)  for  the  augmented  set.  The  first  of  these,  a,  is  a  scaling  parameter  that  determines  the  spread  of  the 
sigma  points  about  the  mean  value  for  the  state,  and  is  usually  set  to  a  small  positive  value  (e.g.,  10‘4<a<l) 
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Table  10.1 1:  Sample  Experimental  Data  for  Determining  Optimum  UKF  Time  Constants 


*s 

II 

r<y> 

UKF  Average 

MSE  of  Rotated 

Reference  Vector 

UKF  Figure  of 

Merit 

1.07 

.04 

le7 

.0010 

.0451 

1.07 

.05 

le7 

.0010 

.0449 

: 

: 

: 

; 

• 

1.07 

.07 

le7 

.0010 

.0449 

; 

: 

• 

: 

• 

1.07 

.10 

le7 

.0010 

.0449 

1.07 

.11 

le7 

.0010 

.0450 

[68].  Not  much  guidance  exists  on  the  selection  of  a  value  for  this  parameter,  but  as  pointed  out  by  [68], 
the  value  of  a  is  not  critical  in  state  estimation  applications  of  the  UKF,  which  is  how  it  is  employed  here. 
Therefore,  a  is  set  to  a  value  of  .01.  The  insensitivity  to  changes  in  a  was  confirmed  through  simulation 
and  the  following  table  illustrates  an  example  of  the  results.  The  p  parameter  corresponds  to  the  type  of 
distribution  expected  of  the  data.  Given  no  information  to  the  contrary,  this  distribution  is  typically 

Table  10.12:  UKF  Performance  Sensitivity  to  Changes  in  a  and  p  Parameters 


a 

p 

II 

UKF  Average 

MSE  of 

Rotated 

Reference 

Vector 

UKF  Figure 

of  Merit 

le-4 

2 

.023 

.20 

le7 

4.48e-4 

6.37e-4 

.05 

2 

le7 

4.48e-4 

6.37e-4 

.1 

2 

le7 

4.48e-4 

6.37e-4 

.5 

2 

.023 

.20 

le7 

4.48e-4 

6.37e-4 

1 

2 

.023 

.20 

le7 

4.48e-4 

6.37e-4 

10 

2 

.023 

.20 

le7 

4.48e-4 

6.37e-4 

.01 

1 

.023 

.20 

le7 

4.48e-4 

6.37e-4 

.01 

5 

.023 

.20 

le7 

4.48e-4 

6.37e-4 
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assumed  to  be  Gaussian,  and  a  value  of  p=2  is  optimum  for  a  Gaussian  distribution  [68].  The  EKF  has  no 
such  parameter,  and  if  the  sensor  or  process  noise  is  indeed  non-Gaussian,  there  may  a  more  suitable  value 
of  p  allowing  the  UKF  to  achieve  better  results  for  the  non-Gaussian  case.  Little  research  has  been  done  in 
this  area,  and  it  warrants  further  investigation.  As  Holt  points  out,  “In  the  practical  case  there  is  usually  no 
a  priori  reason  for  an  assumption  of  a  non-Gaussian  distribution  [34].”  That  assumption  is  made  for  this 
work,  and  P=2  is  used  in  all  cases.  As  evidenced  by  the  data  in  the  table  above,  however,  in  this 
application  the  filter  is  insensitive  to  changes  in  this  parameter,  at  least  over  the  range  tested.  A  third 
parameter  that  is  needed  for  implementation  of  the  UKF  is  k,  as  found  in  equation  (9.56).  This  is  a 
secondary  scaling  parameter  that  is  typically  set  to  3-L  where  L  is  the  dimension  of  the  state  vector  [68], 
In  this  case  L  =  7,  and  the  parameter  k  is  set  to  -4  for  the  original  sigma  set. 

If  an  augmented  set  of  sigma  points  is  used,  as  it  is  in  this  design,  the  parameters  a1,  p\  and  k*  must  also 
be  determined.  These  correspond  to  the  parameters  a,  p,  and  k  described  above  for  the  unaugmented  set, 
and  the  same  guidelines  apply  for  setting  their  values.  For  this  effort  they  are  set  to,  a '=.01,  P  =2,  and 
K,=-l  1. 

10.6.2  Sample  Performance  When  no  Rate  Measurements  are  Available 

As  with  the  EKF  described  in  Section  10.5,  the  UKF  has  been  implemented  both  to  accept  actual  noisy 
rate  measurements  from  rate  sensors  and  to  operate  on  “measurements”  derived  from  differencing 
quaternion  rates  as  described  in  equations  (9.1)  and  (9.2).  This  section  discusses  performance  when  no 
direct  measurements  of  rate  are  available.  As  with  the  EKF  without  rates,  the  differencing  causes  a  one 
time  step  delay  in  the  processing.  As  was  mentioned  earlier,  for  most  applications,  and  assuming  a  small 
time  step,  this  should  not  be  an  issue.  The  software  to  implement  this  filter  is  norates_ukf.m  and  is  found 
in  Appendix  A.  The  data  in  the  following  table  was  generated  for  the  duration  of  simulation  shown  using 
sensor  measurement  standard  deviations  of  1.333  degrees  for  each  axis  of  the  Sun  sensor  and  3.333 
degrees  for  the  magnetometer  measurements.  The  rate  profiles  asymptotically  approach  the  final  values  of 
0.5  rev/min,  0.5  rev/min,  and  225  rev/min  about  the  body  x,  y,  and  z  axes  respectively.  The  final  values 
are  achieved  at  20  seconds.  The  integration  step  size  is  the  standard  0.01  seconds.  These  results  are  from 
processing  the  identical  data  sets  as  processed  for  the  EKF  results  in  Section  10.5,  with  the  rows  from  all 
tables  corresponding  to  the  same  simulation  data.  This  was  done  to  facilitate  straight-across  comparison 
of  performance  between  filter  algorithms.  In  each  case,  the  filter  parameters  have  been  tuned  to  achieve 
the  best  possible  figure  of  merit  for  the  particular  data  set,  and  the  table  values  represent  the  optimum 
performance  for  the  filter,  as  implemented  here,  on  that  set  of  simulated  data.  Here,  again,  <J>^  is  the 
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process  noise,  T  =  T  is  the  time  constant  associated  with  Oi  and  ©2  and  is  the  time  constant 
associated  with  g)3.  The  MSE  and  the  figure  of  merit  are  as  defined  earlier. 

Table  10.13:  Sample  UKF  Performance  When  no  Rate  Measurements  are  Available 


Duration  of 

Simulation 

(seconds) 

^ 6){  ^0)2 

tOh) 

UKF  Average 

MSE  of 

Rotated 

Reference 

Vector 

UKF  Figure 

of  Merit 

40  (set  #1) 

1.07 

.07 

le7 

.0010 

.0449 

40  (set  #2) 

1.05 

.09 

le7 

9.73e-4 

.0440 

60  (set  #3) 

1.01 

.07 

le7 

9.96e-4 

.0373 

60  (set  #4) 

0.99 

.07 

le7 

9.80e-4 

.0376 

The  following  table  displays  a  number  of  “angle”  performance  measures  that  correspond  to  these 
numerical  measures  of  performance.  While  the  figure  of  merit  defined  earlier  is  the  standard  of 
performance  for  relating  one  method  to  another  for  a  given  set  of  data,  these  angle  measures  help  give  an 
intuitive  feel  for  how  well  the  filter  is  performing.  Angle  1  refers  to  the  angle  between  the  body  x  axis 
rotated  using  the  “true”  quaternion  and  the  body  x-axis  rotated  using  the  filter  estimated  quaternion,  or  the 
“error”  angle  for  the  first  axis.  Angle2  and  Angle3  are  similarly  calculated  for  the  body  y  and  z  axes 
respectively,  a  indicates  the  standard  deviation  of  the  error  angle.  The  rows  in  this  table  correspond  to  the 
rows  in  the  previous  table,  coming  from  the  same  simulated  data. 

Table  10.14:  UKF  Angle  Measure  Performance  When  no  Rate  Measurements  are  Available 


Mean  Angie  i 

^Anglel 

Max  Angle  1 

MeanAng!e2 

^  Angle2 

MaXAngle2 

MeanAngie3 

G>Angle3 

MaXAngle3 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

2.76 

1.68 

12.69 

2.76 

1.73 

12.30 

3.23 

1.92 

13.60 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

2.80 

1.68 

11.44 

2.78 

1.70 

13.21 

3.28 

1.89 

12.20 
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10*6.3  Sample  Performance  When  Rate  Measurements  are  Available 

Finally,  the  UKF  has  been  implemented  to  accept  actual  noisy  rate  measurements  from  rate  sensors  in 
rates_ukf.m  found  in  Appendix  A.  Using  rate  sensors  eliminates  the  lag  seen  when  estimating  rotational 
rates  from  differencing  and  results  in  better  estimation  of  both  the  attitude  quaternion  and  the  rotational 
rates.  An  example  of  this  lag  is  seen  in  the  next  two  figures.  These  figures  are  from  a  simulation  of  the 
baseline  case,  and  the  lag  in  the  estimate  for  the  “no  rates”  filter  is  clearly  seen  in  the  first  figure,  and  not 
present  in  the  “with  rates”  filter  results.  The  price  to  be  paid  is  increased  cost  and  complexity  in  order  to 


Quaternion  derived  omega3,  UKF  estimate  of  omega3,  'True"  omega3 


Figure  10.15:  Lag  in  Filter  Rate  Estimate  When  Rate  Measurements  not  Used 

incorporate  the  rate  sensors  for  each  of  the  body  axes  into  the  system.  The  data  in  the  following  table  was 
generated  for  the  duration  of  simulation  shown  using  sensor  measurement  standard  deviations  of  1.333 
degrees  for  each  axis  of  the  Sun  sensor  and  3.333  degrees  for  the  magnetometer  measurements.  The  rate 
profiles  asymptotically  approach  the  final  values  of  0.5  rev/min,  0.5  rev/min,  and  225  rev/min  about  the 
body  x,  y,  and  z  axes  respectively.  The  final  values  are  achieved  at  20  seconds.  The  integration  step  size 
is  the  standard  0.01  seconds.  These  results  are  from  processing  the  identical  data  sets  as  processed  for  the 
EKF  results  in  Section  10.5  and  for  the  UKF  without  rate  measurements  in  the  last  section.  The  rows  from 
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omega3  from  sensor,  UKF  estimate  of  omega3,  "True"  omega3 


Figure  10. 16:  No  Lag  in  Filter  Rate  Estimate  When  Rate  Measurements  are  Used 

all  tables  correspond  to  the  same  simulation  data.  Again,  straight-across  comparison  of  performance 
between  filter  algorithms  is  possible.  In  every  case,  the  filter  parameters  have  been  tuned  to  achieve  the 
best  possible  figure  of  merit  for  the  particular  data  set,  and  the  table  values  represent  the  optimum 


Table  10.15:  Sample  UKF  Performance  When  Rate  Measurements  are  Available 


Duration  of 

Simulation 

(seconds) 

II 

T<*> 

UKF  Average 

MSE  of 

Rotated 

Reference 

Vector 

UKF  Figure  of 

Merit 

40  (set  #  1 ) 

.023 

.20 

le7 

4.48e-4 

6.37e-4 

40  (set  #2) 

.023 

.36 

le7 

4.41e-4 

6.34e-4 

40  (set  #5) 

.023 

.32 

lei  1 

4.43e-4 

6.30e-4 

60  (set  #3) 

.020 

.50 

le7 

3.78e-4 

5.94e-4 

60  (set  #4) 

.022 

.47 

le7 

4.23e-4 

5.85e-4 
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performance  for  the  filter,  as  implemented  in  this  work,  on  that  set  of  simulated  data.  Here,  again,  is 

the  process  noise,  =  T ^  is  the  time  constant  associated  with  ©i  and  ©2  and  T ^  is  the  time  constant 

associated  with  ©3.  The  MSE  and  the  figure  of  merit  are  as  defined  earlier.  The  Table  10.16  displays  a 
number  of  “angle”  performance  measures  that  correspond  to  these  numerical  measures  of  performance. 
While  the  figure  of  merit  defined  earlier  is  the  adopted  standard  of  performance  for  relating  one  method  to 
another  for  a  given  set  of  data,  these  angle  measures  help  give  an  intuitive  feel  for  how  well  the  filter  is 
performing.  Angle  1  refers  to  the  angle  between  the  body  x  axis  rotated  using  the  “true”  quaternion  and 
the  body  x-axis  rotated  using  the  filter  estimated  quaternion,  or  the  “error”  angle  for  the  first  axis.  Angle2 
and  Angle3  are  similarly  calculated  for  the  body  y  and  z  axes  respectively,  a  indicates  the  standard 
deviation  of  the  error  angle.  The  rows  in  this  table  correspond  to  the  rows  in  the  previous  table,  coming 
from  the  same  simulated  data. 

Table  10.16:  UKF  Angle  Measure  Performance  When  Rate  Measurements  are  Available 


Mean  Angie  1 

G  Angle  1 

Max  Angle  l 

MeanAngie2 

t*Angle2 

MaXAngle2 

MeanAngie3 

^Angle3 

MaXAngle3 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

1.84 

6.55 

1.86 

7.21 

2.01 

1.10 

7.12 

1.82 

7.35 

1.85 

7.93 

2.06 

1.12 

7.70 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

N/A 

1.80 

0.98 

6.52 

1.82 

1.00 

6.95 

1.97 

1.06 

7.17 

10.7  Summary  of  Filter  Relative  Performance 

A  small  subset  of  the  overall  simulation  data  used  to  evaluate  the  various  algorithms  has  been  presented  in 
this  chapter.  The  objective  was  to  determine  which  of  the  methods,  those  based  on  a  more  traditional 
extended  Kalman  filter  approach  or  those  based  on  the  relatively  new  unscented  Kalman  filter,  provides 
the  greatest  leverage  with  respect  to  the  low-cost  sounding  rocket  attitude  problem.  Some  of  the  more 
important  constraints  associated  with  this  problem  are  that  it  is  nonlinear,  that  low-cost  approaches  often 
mean  less  sensors  and  that  those  sensors  are  of  lower  accuracy,  and  somewhat  limited  availability  of 
computing  resources.  The  performance  data  from  the  last  several  sections  is  compiled  in  the  following 
table  for  comparison  purposes.  Added  to  what  was  presented  before,  is  the  performance  achieved  using 
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the  quaternions  derived  from  the  Gauss-Newton  error  minimization,  without  filtering.  The  measure  GN- 
MSE  is  generated  using  this  quaternion  to  rotate  the  reference  vectors  and  then  making  a  comparison  with 
the  reference  vectors  rotated  using  the  “true”  quaternion.  These  and  the  rotational  rate  “measurements,” 
either  measured  or  from  differencing,  are  used  to  calculate  the  GN-FOM,  or  “Gauss-Newton  Figure  of 
Merit”  in  the  same  manner  as  the  EKF  and  UKF  figures  of  merit  are  calculated.  With  this  new 
information,  a  true  side-by-side  comparison  of  the  performance  of  each  filter  type,  and  the  unfiltered 
solution,  is  made  possible.  As  before,  each  row  contains  results  from  processing  the  identical  data  set  by 
each  method. 


Table  10.17:  Summary  of  Algorithm  Relative  Performance 


GN- 

MSE 

EKF- 

MSE 

(no 

rates) 

UKF- 

MSE 

(no 

rates) 

EKF- 

MSE 

(with 

rates) 

UKF- 

MSE 

(with 

rates) 

GN- 

FOM 

(no 

rates) 

EKF- 

FOM 

(no 

rates) 

UKF- 

FOM 

(no 

rates) 

GN- 

FOM 

(with 

rates) 

1 

UKF- 

FOM 

(with 

rates) 

Data 

Set#l 

.0010 

.0010 

9.72e-4 

4.48e-4 

37.31 

.0688 

.0449 

.0016 

.0011 

6.37e-4 

Data 

Set  #2 

9.74e-4 

9.7le-4 

9.49e-4 

4.41e-4 

35.56 

.0679 

.0440 

.0016 

.0011 

6.34e-4 

Data 

Set  #3 

9.97e-4 

9.94e-4 

9,96e-4 

9.77e-4 

3.78e-4 

37.06 

.0571 

.0373 

.0016 

.0011 

5.94e-4 

Data 

Set  #4 

9.80e-4 

9.78e-4 

9.80e-4 

9.62e-4 

4.23e-4 

35.86 

.0587 

.0376 

.0016 

.0011 

5.85e-4 

Evaluating  the  figures  of  merit  for  the  various  algorithms  reveals  that  the  UKF  method  using  rate  sensor 
data  is  significantly  better  than  all  others.  In  fact,  the  results  from  data  set  #4,  one  of  the  60  second 
simulations,  indicate  that  the  figure  of  merit  is  improved  by  nearly  47  percent  over  the  nearest  competitor, 
the  EKF  using  rate  sensors!  Clearly  the  performance  of  the  methods  that  do  not  incorporate  rate  sensors  is 
lower,  as  would  be  expected.  It  should  be  pointed  out,  however,  that  these  algorithms  are  indeed 
successful  at  estimating  all  the  states,  albeit  at  a  lower  accuracy  than  the  “with  rates”  algorithms.  As  is 
clearly  shown  in  the  table  above,  both  the  EKF  and  UKF  based  filters  show  huge  improvement  over 
simply  using  the  Gauss-Newton  error  minimization  directly  and  then  differencing  quaternions  to  estimate 
rates.  The  exceptionally  large  figures  of  merit  for  the  Gauss-Newton  case  reflect  the  relatively  poor 
overall  performance  achieved  when  no  processing  of  the  rate  measurements  has  been  done.  Examination 
of  the  GN-MSE  column,  which  does  not  assess  estimation  of  the  rates,  indicates  that  the  attitude 
determination  performance  is  very  good  having  only  been  processed  by  the  Gauss-Newton  routine.  The 
question  for  a  system  designer  then  becomes  one  of  trade-offs,  balancing  the  increased  performance  of 
including  rates  against  the  simplicity  and  lower  accuracy  of  a  “no-rates”  approach.  Referring  to  set  #4 
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again,  the  best  “no  rates”  algorithm  is  the  UKF  again,  displaying  an  approximately  36  percent 
improvement  in  figure  of  merit  over  the  EKF  method  without  rates. 

Examination  of  the  UKF-MSE  (no  rates)  column  at  first  appears  to  indicate  an  aberration  in  that  the  UKF- 
based  algorithm  does  more  poorly  than  the  EKF-based  algorithm  with  respect  to  the  mean  square  error 
criteria.  This  is  an  artifact  of  tuning  to  the  lowest  figure  of  merit,  and  not  to  the  lowest  MSE.  In  this  way 
the  filter  is  optimized  for  estimating  both  the  rotational  rates  and  the  attitude  quaternion.  In  effect,  this  is  a 
best  compromise.  In  every  case  tested,  a  set  of  parameters  were  found  that  yielded  a  lower  MSE  for  the 
UKF  than  that  achievable  by  the  EKF.  This  approach  to  tuning  results  in  a  higher  overall  figure  of  merit, 
but  yields  a  slightly  more  accurate  attitude  solution.  The  choice  of  tuning  approach,  for  best  attitude  or  for 
best  figure  of  merit,  must  be  made  by  the  user,  taking  into  account  the  particular  problem  parameters  (i.e., 
importance  of  attitude  relative  to  rates,  etc.). 

While  the  figures  of  merit  in  the  table  above  clearly  indicate  relative  performance  between  filters,  they  do 
not  provide  an  easily  visualized,  or  intuitive,  measure  of  performance.  The  next  table  summarizes  the 
“error  angles”  associated  with  the  solution  from  each  algorithm.  These  angles  are  calculated  between  the 
body  axes  when  rotated  using  the  true  quaternion  and  the  body  axes  when  rotated  using  the  filter-produced 
quaternion.  While  they  do  not  give  an  indication  of  the  rate  estimation  performance,  they  do  help 
visualize  the  relative  performance  of  the  different  filters  from  a  “pointing  accuracy”  perspective.  Angle  1 
corresponds  to  the  first  body  axis,  and  Angle  2  and  Angle  3  correspond  to  the  second  and  third  axes 
respectively  Here  again,  the  same  relative  performance  is  seen.  The  UKF  filter  using  rates  achieves  much 
better  results  than  does  any  other.  From  the  tabulated  data,  it  is  clear  that  this  filter  produces  lower  mean 
errors  and  errors  with  a  smaller  standard  deviation.  Again,  the  nearest  competitor  is  the  EKF  filter  that 
uses  rates,  and  the  UKF  filter  enjoys  a  significant  performance  advantage.  Evaluating  the  spin  axis 
pointing  error,  angle  3,  the  UKF  performance  is  approaching  an  improvement  of  37  percent  for  data  set  #2 
and  is  approximately  39  percent  better  for  data  set  #4. 

Another  significant  observation  from  these  results  is  that  the  “no  rates”  methods  do  not  yield  much 
improvement  in  terms  of  pointing  accuracy.  This  is  likely  due  to  the  very  simple  embedded  dynamic 
model  and  the  fact  that  the  rates  derived  from  differencing  are  still  very  noisy.  It  seems  clear  that  for  real 
improvement  in  attitude  estimation,  rates  must  be  included  and,  as  demonstrated  above,  the  UKF-based 
algorithm  clearly  outperforms  the  EKF. 
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Table  10.18:  Summary  of  Algorithm  Relative  Error  Angle  Performance 


Data  Set 

#2  (40  sec) 

Mean 

Ancle  1 

dAnelel 

Max 

Ancle  1 

Mean 

Ancle  2 

OAntUl 

Max 

Ancle  2 

Mean 

Ancle  3 

0Atiele3 

Max 

Ancle  3 

Gauss- 

Newton 

2.76 

1.68 

12.70 

2.76 

1.73 

12.31 

3.23 

1.92 

13.61 

EKF 

(no  rates) 

2.75 

1.68 

12.69 

2.76 

1.73 

12.30 

3.23 

1.92 

13.61 

UKF 

(no  rates) 

2.76 

1.68 

12.69 

2.76 

1.73 

12.30 

3.23 

1.92 

13.60 

EKF 

(with  rates) 

2.72 

1.65 

12.61 

2.73 

1.71 

12.26 

3.19 

1.91 

13.59 

UKF 

(with  rates) 

1.84 

1.00 

6.55 

1.86 

1.05 

7.21 

2.01 

1.10 

7.12 

ppfPII 

1111S1 

*  |  , 

igm 

iiigiis 

mm 

ilPfif 

&MMI 

Data  Set 

(60  sec) 

Mean 

Ancle  1 

<LiwleI 

Max 

Ancle  1 

Mean 

Ancle  2 

i lAneU2 

Max 

Ancle  2 

Mean 

Ancle  3 

i lAnele3 

Mss 

Anele  3 

Gauss- 

Newton 

2.80 

1.68 

11.44 

2.78 

1.70 

13.22 

3.28 

1.89 

12.21 

EKF 

(no  rates) 

2.80 

1.68 

11.44 

2.78 

1.69 

13.21 

3.27 

1.89 

12.21 

UKF 

(no  rates) 

2.80 

1.68 

11.44 

2.78 

1.70 

13.21 

3.28 

1.89 

12.20 

EKF 

(with  rates) 

2.77 

1.67 

11.47 

2.74 

1.68 

13.12 

3.24 

1.88 

12.23 

UKF 

(with  rates) 

1.80 

0.98 

6.52 

1.82 

1.00 

6.95 

1.97 

1.06 

7.17 

Given  the  substantial  performance  advantages  associated  with  the  UKF  filter,  with  the  assumption  of 
Gaussian  noise,  the  remainder  of  this  work  will  further  investigate  its  operating  characteristics.  Chapter 
1 1  will  address  the  filter’s  ability  to  deal  with  different  quality  sensors,  biases,  and  data  dropouts. 
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11.0  Filter  Behavior  Under  Other  Than  Nominal  Conditions 

The  development  thus  far  has  considered  the  relative  performance  of  four  different  algorithms  given  an 
“as  expected”  environment.  The  UKF-based  filter  using  rate  measurements  was  determined  to  exhibit  the 
best  overall  performance.  There  are,  however,  a  number  of  deviations  from  this  “as  expected” 
environment  with  which  an  attitude  determination  algorithm  will  have  to  contend.  This  chapter  addresses 
several  of  these  situations  and  evaluates  the  performance  of  the  UKF-based  filter  using  rate  measurements 
under  these  “other  than  nominal”  conditions. 

11.1  Performance  With  Less  Frequent  Measurements 

One  important  practical  concern  when  implementing  any  filtering  algorithm  is  how  its  performance  varies 
with  respect  to  the  frequency  at  which  measurements  are  available.  This  can  have  a  significant  impact  on 
the  number  of  sensors  necessary  to  achieve  the  desired  result  and  the  processing  power/speed  required. 
The  application  of  interest  in  this  work,  attitude  determination  for  sounding  rockets,  assumes  the  use  of 
Sun  sensors,  magnetometers,  and  rate  gyroscopes.  The  magnetometers  and  the  rate  gyroscopes  can 
theoretically  be  polled  at  any  time,  so  measurement  availability  from  these  sensors  does  not  drive  the 
filtering  cycle.  The  Sun  sensor  has  a  defined  field  of  view,  and  measurements  will  only  be  available  when 
the  Sun  is  in  this  field  of  view.  Since  the  vehicle  is  undergoing  rotational  motion,  this  field  of  view 
sweeps  out  a  portion  of  the  sky  at  each  rotation.  Obviously,  how  often  Sun  vector  measurements  are 
available  is  a  function  of  how  many  Sun  sensors  are  employed  and  what  their  field  of  view  characteristics 
are.  This  measurement  frequency  is  also  clearly  a  function  of  the  rotational  motion  of  the  vehicle.  While 
the  detailed  design  of  an  actual  system  requires  many  cost/benefits  analyses  to  determine  the  number  of 
sensors  and  their  placement,  a  number  of  simulations  were  conducted  to  help  characterize  the  performance 
of  this  filtering  algorithm  with  respect  to  the  availability  of  sensor  measurements.  This  analysis  assumes 
that  only  complete  measurement  sets  are  fed  to  the  filter,  meaning  that  Sun  vector,  magnetic  field  vector 
and  body  rate  measurements  are  available  at  the  filtering  time  step.  Performance  is  analyzed  for  two  data 
sets  from  the  baseline  example  at  various  measurement  intervals.  The  intervals  are  chosen,  to  a  very 
rough  approximation,  to  correspond  to  what  would  be  available  from  Sun  sensors  given  the  225  rev/min 
principle  spin  rate  and  one  sensor,  two  sensors,  three  sensors,  etc.  Since  these  intervals  assume  the  sensor 
only  sees  the  Sun  when  it  is  directly  on  the  bore  sight  of  the  sensor,  these  intervals  are  actually  worse  than 
what  might  be  expected  for  an  actual  implementation.  Again,  the  measurement  interval  is  the  result  of 
many  trade  offs,  and  this  analysis  is  meant  to  illustrate  the  trend  with  respect  to  performance  versus 
interval.  In  each  case,  the  parameters  are  tuned  to  the  best  possible  figure  of  merit  and  Table  11.1  below  is 
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Table  11.1:  Performance  at  Varying  Measurement  Interval 


Duration  of 

Simulation 

(seconds) 

Measurement 

Interval  (sec) 

II 

*5 

r<y, 

Gauss- 

Newton 

Figure  of 

Merit 

UKF 

Figure  of 

Merit 

40  (set  #2) 

0.010 

0.023 

0.340 

lel5 

.0016 

6.34e-4 

40  (set  #2) 

0.020 

0.030 

0.240 

lel2 

.0016 

8.57e-4 

40  (set  #2) 

0.040 

0.050 

0.070 

lel2 

.0016 

0.0011 

40  (set  #2) 

0.070 

0.070 

0.060 

lel3 

.0016 

0.0014 

40  (set  #2) 

0.090 

0.080 

0.060 

lel5 

.0016 

0.0015 

40  (set  #2) 

0.135 

0.340 

0.050 

7 

.0016 

0.0016 

40  (set  #2) 

0.270 

0.260 

0.010 

15 

.0017 

0.0017 

;•  -Vv  ri; 

40  (set  #5) 

0.005 

0.018 

0.430 

lel2 

.0016 

4.49e-4 

40  (set  #5) 

0.010 

0.023 

0.320 

lei  1 

.0016 

6.30e-4 

40  (set  #5) 

0.020 

0.031 

0.190 

lel2 

.0016 

8.48e-4 

40  (set  #5) 

0.040 

0.050 

0.070 

lel3 

.0016 

.0012 

40  (set  #5) 

0.070 

0.070 

0.070 

le  13 

.0015 

.0013 

40  (set  #5) 

0.090 

0.085 

0.060 

lel3 

.0016 

.0014 

40  (set  #5) 

0.135 

0.340 

0.050 

116 

.0015 

.0015 

40  (set  #5) 

0.270 

0.355 

0.100 

lei  1 

.0015 

.0015 

a  summary  of  the  results.  Evaluation  of  the  figure  of  merit  results  illustrates  that  for  a  given  sensor 
accuracy,  more  frequent  measurements  yield  a  better  solution.  It  is  also  evident,  that  there  is  a  minimum 
time  interval  at  which  the  UKF  begins  to  provide  a  performance  advantage  over  what  is  available  from 
using  only  the  Gauss-Newton  portion  of  the  algorithm.  For  these  two  sample  data  sets,  this  performance 
advantage  is  evident  at  measurement  intervals  of  0.09  seconds  or  less.  While  there  is  an  improvement 
over  the  performance  of  Gauss-Newton  alone,  substantial  improvement  in  performance  is  not  seen  until 
the  measurement  interval  is  closer  to  that  predicted  by  the  time  step  discussion  in  an  earlier  section.  Given 
the  motion  simulated  here,  a  sample  rate,  or  filter  time  step,  on  the  order  of  .01  sec  is  expected  to  generate 
good  results,  and  that  is  the  case  in  this  data.  This  increase  in  the  quality  of  the  solution  with  more 
frequent  measurements  matches  the  intuitive  assessment,  as  one  would  expect  that  more  information 
would  yield  a  better  solution.  In  fact,  as  described  in  Chapter  7,  this  is  not  true  in  the  general  data  fusion 
sense,  since  improperly  combining  additional  information  can  sometimes  lead  to  a  worse  solution  than  if 
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the  new  information  had  not  been  included  [61].  Fortunately,  since  the  filter  is  designed  to  optimally 
estimate  the  solution,  more  information  does  indeed  provide  a  better  solution  in  this  case.  This  type  of 
assessment  can  be  used  by  a  system  designer  as  one  piece  of  information  in  his  decision  on  how  many 
sensors  should  be  included. 

The  analysis  in  this  section  was  accomplished  by  varying  the  measurement  interval  while  holding  the 
sensor  accuracy  constant.  The  next  section  provides  examples  of  performance  as  the  sensor  accuracies  are 
changed. 

11.2  Performance  With  Sensors  of  Greater  and  Lesser  Accuracy 

The  development  of  the  filter  algorithms  and  testing  thus  far  has  centered  on  baseline  examples  that  use 
representative  values  for  the  sensor  accuracies.  These  measurement  standard  deviations,  1.333  degrees  for 
the  Sun  sensor,  3.333  degrees  for  the  magnetometer,  and  0.333  rev/min  for  the  rate  sensors,  were 
determined  in  Chapter  6  as  a  best  assessment  of  what  is  likely  to  be  available  to  a  low-cost  system 
designer.  In  this  section,  representative  performance  of  the  algorithm  given  measurements  of  better,  and 
worse,  accuracy  will  be  evaluated.  To  evaluate  performance  with  the  availability  of  better  sensors,  a  Sun 
sensor  with  a  measurement  standard  deviation  of  0.5  degrees,  a  magnetometer  with  a  measurement 
standard  deviation  of  1.0  degrees  and  rate  sensors  with  measurement  standard  deviations  of  0.25  rev/min 
were  simulated.  For  the  case  of  worse  sensors,  measurement  standard  deviations  of  5  degrees,  10  degrees 
and  1  rev/min  were  used.  The  simulated  profile  matches  that  of  the  baseline  example,  with  an  asymptotic 
approach  to  final  rates  of  0.5  rev/min  about  the  body  “x”  axis,  0.5  rev/min  about  the  body  “y”  axis,  and 
225  rev/min  about  the  body  “z”  axis.  For  each  case,  the  filter  parameters  are  tuned  for  the  optimum 
overall  figure  of  merit,  as  has  been  done  consistently  throughout  this  work.  Tables  11.2  and  11.3 
summarize  the  results  from  these  simulations.  While  the  figure  of  merit  is  the  adopted  overall 
performance  measure,  the  mean  values  for  the  “pointing  accuracy”  angles  corresponding  to  each  of  the 
body  axes  are  also  given  to  help  visualize  the  relative  performance  as  a  function  of  sensor  accuracy.  As 
expected,  the  better  the  sensor,  the  better  the  solution.  What  is  of  note,  however,  is  that  the  filter  provides 
the  greatest  improvement  for  scenarios  using  less  accurate  sensors.  The  improvement  in  figure  of  merit 
for  the  first  case  above  is  on  the  order  of  61  percent.  For  the  most  accurate  sensor  case,  it  is  approximately 
58  percent.  For  the  least  accurate  sensors  simulated  above,  the  improvement  is  on  the  order  of  a  huge  76.7 
percent.  So,  while  the  filter  works  well  with  any  of  these  sensors,  it  appears  to  be  especially  well-suited  to 
lower  performing  sensors,  like  those  envisioned  for  this  application.  Having  examined  the  filter  behavior 
with  respect  to  changes  in  measurement  interval  and  changes  in  sensor  accuracy,  the  next  section 
investigates  the  impact  of  the  loss  of  measurements  on  the  algorithm. 
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Table  11.2:  Sample  Filter  Performance  for  Sensors  of  Varying  Accuracy 


Simulation 

II 

Gauss-Newton 

Figure  of  Merit 

UKF  Figure 

of  Merit 

40  (asymptotic, 
1.333°, 

3.333°, 

0.333  rpm) 

0.023 

0.32 

lei  1 

0.0016 

6.30e-4 

40  (asymptotic, 
0.5°, 

1.0°, 

0.25  rpm) 

0.022 

0.24 

lei  1 

6.91e-4 

2.93e-4 

40  (asymptotic, 

5°, 

10°, 

1.0  rpm) 

0.046 

0.17 

lel3 

0.0146 

0.0034 

Table  1 1.3:  Error  Angle  Comparison  as  a  Function  of  Sensor  Accuracy 


Simulation 

*s 

V? 

II 

*5 

To>i 

GN 

Mean 

Error 

Angle 

1  (deg) 

GN 

Mean 

Error 

Angle 

2  (deg) 

GN 

Mean 

Error 

Angle 

3 (deg) 

UKF 

Mean 

Error 

Angle 

1  (deg) 

UKF 

Mean 

Error 

Angle 

2  (deg) 

UKF 

Mean 

Error 

Angle 

3  (deg) 

40 

(asymptotic, 
1.333°, 
3.333°, 
0.333  rpm) 

0.023 

0.32 

lei  1 

2.81 

2.80 

3.33 

1.82 

1.85 

2.06 

40 

(asymptotic, 

0.5°, 

1.0°, 

0.25  rpm) 

0.022 

0.24 

lei  l 

0.93 

0.92 

1.04 

0.88 

0.87 

0.97 

40 

(asymptotic, 

5°, 

10°, 

1.0  rpm) 

0.046 

0.17 

lel3 

8.86 

8.92 

9.99 

4.47 

4.53 

4.58 

1L3  Behavior  in  the  Event  of  a  Loss  of  Measurements 

Another  situation  that  may  occur  is  the  loss  of  measurements  for  some  period  of  time  due  to  telemetry 
problems,  on-board  hardware  problems,  or  a  host  of  other  reasons.  While  total  loss  of  data  is  an 
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unrecoverable  situation,  lost  measurements  or  short  periods  without  data  may  occur.  To  be  useful,  in  this 
event,  the  filter  algorithm  should  be  well-behaved  and  recover  gracefully  to  provide  acceptable  estimates. 
This  section  evaluates  the  performance  of  the  UKF  algorithm  under  these  conditions.  Six  different 
approaches  were  investigated  for  dealing  with  a  loss  of  measurements.  Approach  1  consists  of  simply 
setting  the  Gauss-Newton  produced  quaternion  back  to  [0  0  0  1]  and  letting  the  rates  be  processed  as  being 
equal  to  zero  for  each  time  step  where  measurements  are  missing.  Approach  2  involves  setting  the 
quaternion  “measurement”  equal  to  the  previous  “measurement”  and  leaving  the  rotational  rates  equal  to 
zero  at  time  steps  that  do  not  have  a  measurement.  Approach  3  sets  the  quaternion  “measurement”  to  the 
last  “measurement,”  but  now  sets  the  rate  measurements  to  the  last  rate  measurement  for  each  time  step 
that  has  a  measurement  missing.  The  fourth  approach  implements  approach  3,  but  also  makes  a  change  in 
the  filter  code.  The  “measurement”  vector  is  set  equal  to  the  predicted  observation  vector,  in  essence 
making  the  filter  residual  equal  to  zero,  thereby  weighting  the  measurement  at  zero  and  making  the  next 
state  estimate  equal  to  the  projected  state.  Approach  5  sets  the  Gauss-Newton  quaternion  “measurement” 
equal  to  the  last  quaternion  estimate  out  of  the  filter  and  the  rate  measurements  equal  to  the  last  rate 
estimates  out  of  the  filter.  Finally,  approach  6  implements  approach  5,  but  also  sets  the  “measurement” 
vector  in  the  filter  equal  to  the  predicted  observation  vector.  Although  more  sophisticated  methods  may 
certainly  be  devised,  each  of  the  approaches  described  here  allow  the  filter  to  successfully  continue 
estimating  the  state  vector  despite  a  measurement  loss.  Each  was  tested  against  the  baseline  example  with 
a  one  second  loss  of  data  in  the  center  of  the  “knee”  in  the  rate  profiles.  The  data  “drop  out”  begins  at  9.5 
seconds  and  continues  to  10.5  seconds.  Table  1 1.4  summarizes  the  results.  Clearly  methods  5  and  6  are 

Table  1 1.4:  Sample  Performance  With  One  Second  Measurement  Loss 


Design 

Approach 

Duration  of 

Simulation 

(seconds) 

II 

*5 

Gauss- 

Newton 

Figure  of 

Merit 

(With 

Dropout) 

UKF 

Figure 

of  Merit 

(No 

Dropout) 

UKF 

Figure  of 

Merit 

(With 

Dropout) 

Approach  1 

40  (set  #2) 

0.023 

0.360 

le7 

3.94 

6.34e-4 

0.4874 

Approach  2 

40  (set  #2) 

0.023 

0.360 

le7 

3.94 

6.34e-4 

3.850 

Approach  3 

40  (set  #2) 

0.023 

0.360 

le7 

0.0110 

6.34e-4 

0.0102 

Approach  4 

40  (set  #2) 

0.023 

0.360 

le7 

0.0110 

6.34e-4 

0.0014 

Approach  5 

40  (set  #2) 

0.023 

0.360 

le7 

0.0017 

6.34e-4 

8.25e-4 

Approach  6 

40  (set  #2) 

0.023 

0.360 

le7 

0.0017 

6.34e-4 

8.25e-4 
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significantly  better  methods,  with  performance  approaching  that  of  the  case  with  no  dropouts.  A  one 
second  dropout  is  not  likely,  but  serves  to  illustrate  that  the  algorithm  can  be  made  robust  with  respect  to 
loss  of  measurements.  Figures  11.1  through  1 1.10  illustrate  the  filter  estimates  of  each  state  before,  during 
and  after  the  one  second  dropout  from  9.5  to  10.5  seconds.  While  there  is  a  slight  degradation  in 
performance,  as  determined  by  the  figures  of  merit,  relative  to  the  run  without  a  dropout,  the  filter  does  not 
diverge  or  fail  to  operate  and  continues  to  provide  very  good  estimates. 


0.25 


omegal  measurement,  UKF  estimate  of  omegal,  "True"  omegal 


15  20  25 

time  (sec) 


Figure  11.1:  UKF  Estimate  of  Through  One  Second  Measurement  Dropout 


time  (sec) 


Figure  1 1.2:  Close-up  of  UKF  Estimate  of  a>i  Through  One  Second  Measurement  Dropout 


Figure  1 1.3:  UKF  Estimate  of  c o2  Through  One  Second  Measurement  Dropout 
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omega2  measurement,  UKF  estimate  of  omega2,  "True"  omega2 
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Figure  1 1.4:  Close-up  of  UKF  Estimate  of  co2  Through  One  Second  Measurement  Dropout 


omega3  measurement,  UKF  estimate  of  omega3,  "True"  omega3 


Figure  1 1.5:  UKF  Estimate  of  co3  Through  One  Second  Measurement  Dropout 
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omega3  measurement,  UKF  estimate  of  omega3,  True"  omega3 


Figure  1 1.6:  Close-up  of  UKF  Estimate  of  0)3  Through  One  Second  Measurement  Dropout 


UKF  estimate  of  ql 


Figure  1 1.7:  Close-up  of  UKF  Estimate  of  qj  Through  One  Second  Measurement  Dropout 
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UKF  estimate  of  q2 


Figure  1 1.8:  Close-up  of  UKF  Estimate  of  q2  Through  One  Second  Measurement  Dropout 
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Figure  1 1.9:  Close-up  of  UKF  Estimate  of  q3  Through  One  Second  Measurement  Dropout 
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UKF  estimate  ofq4 


Figure  1 1.10:  Close-up  of  UKF  Estimate  of  q4  Through  One  Second  Measurement  Dropout 
11.4  Impact  of  a  Sensor  Bias 

One  of  the  more  insidious  situations  that  may  be  encountered  is  a  bias  on  one  or  more  of  the 
measurements.  When  this  occurs,  all  indications  may  be  that  the  filter  is  operating  properly,  when  in  fact 
the  solution  is  being  corrupted  at  each  step.  While  the  filter  does  not  diverge  or  cease  to  function,  the 
quality  of  the  solution  may  be  in  question.  Unfortunately  there  is  no  straightforward  solution  to  such  a 
situation.  Especially  in  the  case  where  there  are  no  redundant  sensors,  there  will  be  no  real  way  to  tell  if 
the  filter  is  providing  realistic,  but  incorrect  answers.  One  possible  mitigating  approach  is  to  have  several 
sensors  that  may  be  compared.  In  this  case,  if  there  are  at  least  three,  a  majority  voting  scheme  might  be 
employed  to  determine  if  one  sensor  has  a  bias  or  other  problem.  If  such  a  sensor  could  be  identified,  its 
input  to  the  algorithm  could  be  eliminated.  This  might  be  possible,  for  instance,  if  three  Sun  sensors  were 
employed  to  allow  for  more  frequent  measurements,  as  discussed  in  Section  11.1.  If  one  of  these  sensors 
was  identified  as  problematic,  its  input  might  be  disregarded,  with  a  resulting  gain  in  performance  due  to 
eliminating  the  bias.  This  performance  gain,  however,  could  possibly  be  offset  by  a  negative  impact  due 
to  an  increased  measurement  interval.  The  net  effect  on  performance  would  depend  on  the  particular 
system  under  consideration.  Unfortunately,  the  inclusion  of  redundant  sensors  poses  a  number  of 
problems  with  respect  to  the  constraints  on  a  low-cost  system  approach.  To  evaluate  the  impact  on 
performance  of  the  UKF  filter  when  a  bias  is  present  on  a  sensor,  three  situations  were  evaluated.  First, 
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one  component  of  the  Sun  vector  measurement  was  given  a  constant  bias.  Second,  a  constant  bias  was 
imposed  on  one  component  of  the  magnetic  field  vector  measurement.  Finally,  one  rate  sensor  had  a 
constant  bias  added  to  its  measurement.  Again,  the  baseline  40  second  example  was  utilized  for 
comparison  purposes.  Table  11.5  summarizes  the  results  of  this  testing 

Table  1 1.5:  UKF  Filter  Performance  With  Sensor  Bias 


Duration 

of 

Simulation 

(seconds) 

Bias  Imposed 

on  Simulated 

Measurement 

II 

*5 

to>i 

Gauss- 

Newton 

Figure 

of  Merit 

(No 

Bias) 

Gauss- 

Newton 

Figure 

of  Merit 

(With 

Bias) 

UKF 

Figure 

of 

Merit 

(No 

Bias) 

UKF 

Figure 

of 

Merit 

(With 

Bias) 

40  (set  #2) 

+0.1  on  “x” 

component 

of  Sun  vector 

measurement 

0.023 

0.36 

lei 

.0016 

.0021 

6.34e-4 

.0013 

40  (set  #2) 

+0.1  on  “y” 

component 

of  mag  field 

vector 

measurement 

0.023 

0.36 

le7 

.0016 

.0023 

6.34e-4 

.0015 

40  (set  #2) 

+0. 1  rev/min 

on  “x”  axis 

rate 

measurement 

0.023 

0.36 

le7 

.0016 

.0047 

6.34e-4 

.0034 

Another  possible  bias  to  be  aware  of,  which  was  not  simulated,  would  be  introduced  by  errors  in  the 
reference  models.  For  example,  this  would  occur  if  the  magnetic  field  vector  was  perturbed  by  electrical 
currents  within  the  rocket.  This  could  potentially  cause  a  bias  on  all  three  of  the  magnetometer 
measurement  axes.  From  the  results  in  Table  11.5,  it  is  clear  that  a  constant  bias  is  a  troubling 
development.  In  these  cases,  the  filter  continues  to  successfully  estimate  the  state  vector,  albeit  with 
greater  error.  There  is  no  divergence  or  other  tell-tale  indication  of  a  problem.  From  a  positive 
perspective,  the  filter  continues  to  operate  normally  and  there  is  no  catastrophic  result.  Depending  upon 
the  bias,  the  impact  may  be  great  or  small.  The  results  from  the  third  trial  above  indicate  poor  overall 
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results,  but  in  fact,  this  bias  on  the  rate  sensor  has  a  small  impact  on  the  estimation  of  the  quaternion. 
Despite  the  accurate  attitude  determination,  the  poor  rate  estimate  figures  into  the  overall  performance 
measure  and  the  overall  figure  of  merit  suffers.  The  first  two  trials  were  for  biases  on  attitude  sensors,  and 
these  translate  directly  into  performance  impacts  on  the  attitude  determination.  Short  of  adding  redundant 
sensors,  there  is  no  obvious  solution  to  this  problem.  Therefore,  significant  emphasis  should  be  placed  on 
testing  the  sensors  before  launch,  and  characterizing  any  measurement  biases  that  may  be  present. 

11.5  Performance  When  Actual  Motion  Is  Different  than  Predicted  Motion 

In  addition  to  loss  of  measurements,  biases,  and  the  issues  surrounding  the  accuracy  of  the  sensors,  the 
motion  encountered  by  the  rocket  may  be  very  different  from  the  “nominal”  motion  expected  and  for 
which  the  filter  parameters  were  tuned.  It  is  important  to  understand  what  the  filter  performance  is  like  in 
such  a  situation.  Fortunately,  the  error  minimization  portion  of  this  algorithm  makes  it  very  robust  to 
changes  in  the  character  of  the  motion,  with  respect  to  determining  the  attitude.  Extensive  simulation  has 
demonstrated  that  the  filter  is  able  to  accurately  estimate  the  orientation  of  the  rocket  even  for  motion 
radically  different  than  that  for  which  the  filter  parameters  were  tuned.  In  these  cases,  what  suffer 
significantly  are  the  estimates  of  the  body  rates.  As  an  example,  simulations  were  conducted  using  40 
second  runs  with  the  default  parameters  and  the  optimum  process  noise  and  time  constants  from  the 
baseline  example.  The  rate  profiles  were  changed  from  rates  that  asymptotically  approached  0.5  rev/min 
about  the  body  “x”  axis,  0.5  rev/min  about  the  body  “y”  axis  and  225  rev/min  about  the  body  “z”  axis  to 
profiles  that  simulate  significantly  different  motion.  In  this  manner,  the  filter  performance  is  evaluated 
when  the  parameters  are  tuned  for  one  expected  motion  profile,  but  the  filter  encounters  something 
different.  The  first  trial  uses  the  same  final  rotation  rates  as  the  baseline  example,  but  in  this  case  they  are 
fixed,  instead  of  asymptotically  approaching  the  final  values.  The  second  trial  simulates  motion  with  fixed 
rates  of  30  rev/min  about  each  body  axis.  This  translates  into  motion  represented  by  a  rate  of  59.96 
rev/min  about  the  (1,1,1)  axis  in  the  body  frame.  The  third  example  presented  here  corresponds  to  a 
change  of  principle  spin  axis  from  the  “z”  axis  to  the  “x”  axis,  with  fixed  rates  of  30  rev/min  about  the 
body  “x”  axis,  0  rev/min  about  the  body  “y”  axis,  and  5  rev/min  about  the  body  “z”  axis.  Trial  4  again  has 
rates  of  30  rev/min  about  each  axis,  but  now  these  are  approached  asymptotically  as  in  the  baseline 
example.  Finally,  trial  5  is  the  asymptotic  case  using  rates  of  30  rev/min  about  the  body  “x”  axis,  0 
rev/min  about  the  body  “y”  axis,  and  5  rev/min  about  the  body  “z”  axis.  These  motions  vary  significantly 
from  that  of  the  baseline  example,  and  the  results  of  the  simulation  and  filtering  are  summarized  in  Table 

11.6  below.  Since  a  different  random  noise  component,  but  one  having  the  same  standard  deviation,  is 
added  during  each  simulation,  the  data  sets  used  in  these  cases  are  not  identical  to  the  40  second  baseline 
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Table  1 1.6:  UKF  Filter  Performance  During  Unpredicted  Motion 


40  Second 

Simulation 

Gauss- 

Newton  CO 

Performance 

Gauss- 

Newton 

MSE  for 

Rotated 

Vectors 

Gauss- 

Newton 

Figure 

of  Merit 

UKF© 

Performance 

UKF 

MSE 

for 

Rotated 

Vectors 

UKF 

Figure 

of  Merit 

UKF 

Figure  of 

Merit 

(nominal 

asymptotic) 

Trial  1 

(fixed 

©x=0.5  rpm 

ffly=0.5  rpm 
<oz=225 

rpm) 

0.0012 

9.92e-4 

0.0016 

2.43e-4 

4.39e-4 

5.02e-4 

6.34e-4 

Trial  2 

(fixed 

cdx=30  rpm 

o)y=30  rpm 

oz=30  rpm) 

0.0012 

0.0010 

0.0016 

0.0189 

4.60e-4 

.0189 

6.34e-4 

Trial  3 

(fixed 

cdx=30  rpm 

coy=0  rpm 

0)z=5  rpm) 

0.0012 

0.0010 

0.0016 

0.0098 

4.52e-4 

0.0098 

6.34e-4 

Trial  4 

(asymptotic 

<ox=30  rpm 

©y=30  rpm 
©z=30  rpm) 

0.0012 

9.77e-4 

0.0016 

0.0159 

9.77e-4 

0.0159 

i 

6.34e-4 

Trials 

(asymptotic 

O)x=30  rpm 

(Oy-0  rpm 

<oz=5  rpm) 

0.0012 

9.92e-4 

0.0016 

0.0081 

4.44e-4 

0.0081 

6.34e-4 
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example.  However,  the  values  for  the  “nominal”  motion  are  typical  of  the  baseline  example,  and  adequate 
for  a  qualitative  comparison  of  performance.  As  noted  earlier,  and  made  clear  by  these  results,  the 
orientation  of  the  rocket  is  always  estimated  at  close  to  the  same  accuracy  as  achieved  when  processing 
the  expected  motion.  For  the  case  of  fixed  rates  with  the  same  final  values  as  the  baseline  case,  the  filter 
performance  is  even  better  for  the  fixed  rate  case  than  for  the  asymptotic  case,  due  to  the  ease  of 
estimating  the  simpler,  non-accelerating  motion.  In  cases  where  the  motion  is  characterized  by  different 
final  rates,  however,  the  overall  figure  of  merit  is  severely  degraded,  and  this  is  driven  by  the  much  less 
accurate  estimation  of  the  body  rates.  One  possible  fix,  if  such  degradation  in  performance  can  be 
detected,  is  to  increase  the  process  noise  or  the  time  constants  used  by  the  filter.  While  the  finely  tuned 
parameters  that  are  optimized  for  the  expected  motion  produce  the  best  results  under  ideal  conditions,  an 
actual  implementation  might  warrant  investigating  the  use  of  less-than-optimum  values  in  order  to  achieve 
greater  robustness  in  the  face  of  unexpected  events.  The  next  section  examines  the  possibility  of  detecting 
such  an  occurrence,  what  might  be  done  to  compensate  for  it,  and  applies  such  an  anomaly  detection 
method  to  one  of  the  trials  investigated  here. 

11.6  Determining  if  the  Filter  is  Operating  Correctly 

In  a  simulation  scenario  when  the  truth  is  readily  available,  it  is  straightforward  to  compare  the  results 
coming  out  of  the  filter  to  the  truth  and  assess  performance  of  the  algorithm.  When  evaluating  filter  error 
in  a  simulated  environment,  the  error  in  the  estimate  of  each  state  is  expected  to  fall  within  the  theoretical 
bounds  at  least  68  percent  of  the  time  [17].  In  effect,  we  are  using  the  expectation  that  in  a  Gaussian 
distribution  we  expect  68  percent  to  fall  within  la,  or  one  standard  deviation,  of  the  mean.  For  a  Kalman 
filter-based  algorithm,  this  theoretical  boundary  is  found  in  the  covariance  matrix,  P,  which  is  calculated 
as  part  of  the  iterative  process  and  is  available  at  each  time  step.  P  is  the  expected  value  of  the  difference 
between  the  actual  state  vector  and  the  estimate  of  the  state  vector  times  the  transpose  of  this  quantity. 
The  mathematical  representation  is 

P  =  £[(*  -  x)(x  -  x)^  ] .  (11.1) 

Since  the  variance  of  a  random  variable  can  be  expressed  as  [17] 

a2  =  e([x-E(x)]2'J  =  E(x2)-E2(x)  (11.2) 

the  elements  of  the  diagonal  covariance  matrix  may  be  treated  as  the  variances  of  the  error  in  the  estimates 
of  the  corresponding  state  vector  elements.  Taking  the  square  root  of  the  diagonal  elements,  then,  yields 
the  predicted  standard  deviations  of  the  error  in  the  estimates  of  the  states.  It  is  this  “+/-  la  value”  at  each 
time  step  that  forms  the  theoretical  boundary  of  the  error  in  the  estimate  that  is  seen  in  many  of  the  figures. 
Once  the  algorithm  is  implemented,  however,  the  “truth”  is  not  readily  available.  Since  the  filter  may  be 
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operating  in  what  appears  to  be  a  nominal  fashion,  but  may  be  producing  incorrect  results,  it  would  be 
very  helpful  to  have  a  flag,  as  such,  that  would  indicate  a  problem  with  the  algorithm.  Such  a  flag  could 
then  be  used  as  a  trigger  to  modify  the  parameters  in  the  algorithm  or  to  resort  to  a  secondary  method  that 
may  not  be  as  accurate,  but  that  is  not  subject  to  whatever  is  causing  problems  with  the  primary  algorithm. 

One  such  “flag”  that  is  sometimes  used  for  Kalman  filters  operating  in  real  world  applications  is  an 
evaluation  of  the  “residual”.  The  residual  is  the  quantity  that  is  multiplied  by  the  Kalman  gain  when 
determining  the  state  estimate.  As  seen  in  equation  (9.46)  for  the  EKF  and  again  in  equation  (9.73)  for  the 
UKF,  the  residual  is  the  difference  between  the  projected  observations  and  the  actual  observations  based 
on  sensor  measurements.  Unlike  the  true  value  of  the  states,  both  of  these  quantities  are  available  during 
real  world  operation.  As  Zarchan  points  out 

If  a  theoretical  prediction  of  the  residual  can  be  derived,  we  can  say  that  the  filter  is 
working  properly  if  the  measured  residual  falls  within  the  theoretical  bounds  68%  of  the 
time.  If  the  residual  continually  exceeds  the  theoretical  bounds  by  a  large  factor  (i.e., 
three),  we  can  conclude  that  the  filter  is  diverging  and  that  some  action  is  required  to 
prevent  complete  catastrophe  (i.e.,  add  more  process  noise  or  switch  to  higher-order 
filter).  If  the  residual  instantly  exceeds  the  theoretical  threshold  bounds  by  a  large  value, 
the  data  might  be  bad,  and  we  might  want  to  ignore  that  measurement  [17]. 

Development  and  successful  operation  of  this  form  of  “anomaly  detection”  is  demonstrated  in  [17]  as  an 
effective  means  for  detecting  divergence  in  a  polynomial  Kalman  filter.  The  development  from  [17] 
begins  with  the  linear  Kalman  filter  equation  for  the  state  estimate 

h=*kh-\+Kk^k~H*kh- 1>  (1L3> 

where  <X>,  x ,  K,  and  H  are  as  defined  in  Chapter  8.  It  should  be  noted  that,  unlike  the  linear  Kalman  filter 
for  which  this  development  takes  place,  the  EKF  and  UKF  do  not  use  the  state  transition  matrix,  <E>,  to 
propagate  states  forward.  It  does  appear  in  the  EKF  formulation  as  seen  in  Chapter  8,  but  is  used  for 
determination  of  the  various  matrices  leading  up  to  calculating  the  Kalman  gain,  not  for  state  propagation. 
The  UKF  formulation  uses  a  different  approach  for  determination  of  these  matrices  and  the  Kalman  gain 
that  does  not  rely  on  O.  From  (1 1.3),  it  is  evident  that  the  residual  is  equal  to 

residual  =  z k  -  .  (11.4) 

For  the  case  where  the  system  has  a  linear  measurement  equation,  the  observation  Zk  is  found  using 

zk  =HrJt+vife  (11.5) 

where  the  next  state,  in  the  linear  case,  is  obtained  from 


xk=*kh-\+wk- 


(11.6) 
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Substituting  (11.4)  and  (11.5)  into  (11.6)  yields  an  expression  for  the  residual  of 

residual  =  //<*>***-!  +  Hwk  +  vk~  mk*k  _  i  • 


(11.7) 


Combining  like  terms  produces 

residual  =  ^  +  +  v^  .  (11.8) 

Using  the  definition  of  a  covariance,  the  covariance  of  the  residual  is  found  by  multiplying  (11.8)  by  its 
transpose  and  taking  the  expectation  of  both  sides.  Zarchan  points  out  the  necessary  assumptions  that  “the 
process  and  measurement  noise  are  not  correlated  and  that  neither  noise  is  correlated  with  the  state  or  its 
estimate  [17].”  Given  these  assumptions,  the  covariance  of  the  residual  is  found  to  be 

E(residual* residual^ )  =  ^  ~*k-l^Xk  1 


Because 


+  HE(w  wT)HT  +  Ely  vT ) . 

#C  tC  fC  rv 


Pk  =  -  **-l  )(**-!  "**-1  )T  ] 


Qk  =E(wkwk) 
Rk  =  E(vkvl ) 

the  covariance  of  the  residual  can  be  reduced  initially  to 


E (residual  *  residual T )  =  H®k  T  +  HQ^  H  T  + 


and  then  eventually  to 


E(residual  *  residual T )  =  H(Q>kPd>Tk  +  Qk  )HT  +  R  . 


(11.9) 

(11.10) 

(11.11) 

(11.12) 

(11.13) 

(11.14) 


As  defined  in  Chapter  8,  the  covariance  matrix  before  the  update  is  equal  to 


Mk=<t>kPk<S>Tk+Qk. 


(11.15) 


Substituting  (11.15)  into  (11.14)  yields 

E(residual  *  residual)^  =HM  +R  (11.16) 

k  k 

where  H  is  the  measurement  matrix,  M  is  the  covariance  matrix  before  the  update,  and  R  is  the 
measurement  noise  matrix  as  defined  in  Chapter  8.  While  this  development  is  based  on  the  linear  Kalman 
filter  structure,  each  of  the  matrices  in  the  final  expression  is  also  available  in  the  UKF  formulation. 
Especially  for  this  application  where,  despite  the  nonlinearities  in  the  problem,  the  measurement  matrix  H 
is  essentially  the  identity  matrix,  it  is  hopeful  that  (11.16)  should  provide  similar  warning  of  divergence,  or 
other  serious  breakdown,  in  the  operation  of  the  filter.  To  determine  if  this  is  true,  simulations  were  run  in 
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which  the  filter  is  obviously  working  properly  and  others  where  it  is  not.  The  following  two  figures  show 
plots  from  one  of  these  simulations  of  the  error  in  the  state  estimate  and  of  the  “residual  test”  for  one  state 
from  the  nominal  case.  Here  it  is  clear  that  both  the  error  in  the  state  estimate,  which  is  available  only  in 


error  in  UKF  estimate  of  q2 


Figure  11.11:  Sample  Error  in  State  Estimate  When  Filter  is  Working  Properly 


simulation  when  the  true  value  of  the  state  is  known,  and  the  residuals  corresponding  to  this  state  fall 
within  the  theoretical  bounds  at  least  68  percent  of  the  time.  This  indicates  nominal  operation  of  the  filter. 
Figures  11.13  and  11.14  are  for  the  same  state,  when  the  filter  is  not  operating  properly.  This  anomalous 
operation  is  generated  by  using  the  same  baseline  simulation  as  for  the  earlier  figures,  but  extending  the 
measurement  interval  to  0.1  seconds  versus  0.01  seconds  without  a  corresponding  retuning  of  the  filter 
process  noise.  The  first  of  the  two  shows  that  the  error  in  the  state  estimate  does  not  satisfy  the  68  percent 
criteria,  indicating  a  problem.  Figure  11.14  demonstrates  that  the  filter  residuals  also  violate  the  68 
percent  rule  with  respect  to  their  theoretical  boundaries,  thereby  also  indicating  a  problem  has  occurred. 
Note  that  the  residuals  begin  to  violate  the  theoretical  bounds  at  approximately  the  same  time  in  the  run  as 
the  state  estimate  error  begins  to  deviate.  Since  the  elements  needed  to  produce  the  second  plot  are 
available  without  apriori  knowledge  of  the  true  state  value,  this  suggests  a  promising  approach  to 
evaluating  the  UKF  filter  for  proper  operation  and  serving  as  a  “flag”  in  real  world  operation. 
Unfortunately,  in  cases  where  the  filter  is  not  operating  at  its  best,  but  is  not  experiencing  catastrophic 
failure,  the  residual  test  may  not  be  a  definitive  indicator  for  whether  or  not  to  switch  filter  parameters  or 
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residual  for  q2 


time  (sec) 


Figure  1 1.12:  Sample  Filter  Residual  for  One  State  When  Filter  is  Working  Properly 


Figure  11.13:  Sample  Error  in  State  Estimate  When  Filter  is  Working  Improperly 
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Figure  1 1.14:  Sample  Filter  Residual  for  One  State  When  Filter  is  Working  Improperly 


methods.  The  bias  problem  discussed  in  an  earlier  section  might  be  one  example.  Based  on  the 
parameters  of  a  particular  problem,  the  system  designer  would  need  to  establish  threshold  values  to  serve 
as  triggers  for  making  such  a  determination.  To  illustrate  what  this  type  of  anomalous  filter  performance 
means  in  terms  of  attitude  determination,  the  following  two  figures  show  plots  of  the  error  angle  for  the 
first  body  frame  axis  when  the  filter  is  working  as  expected,  and  when  performing  anomalously.  As 
expected,  the  error  angle  increases  rapidly  once  the  filter  estimates  begin  to  diverge.  The  plots  in  this 
section  illustrate  the  performance  for  only  one  of  the  states  and  the  error  for  only  one  body  axis.  When 
implementing  an  error  detection  method,  such  as  that  described  in  this  section,  the  residuals  corresponding 
to  each  state  should  be  evaluated  since  the  filter  may  continue  to  adequately  estimate  certain  states  while 
failing  to  correctly  estimate  others.  Another  example  where  this  method  certainly  works  is  the  case  where 
the  motion  is  other  than  expected,  as  discussed  in  the  previous  section.  As  an  illustration,  the  data  set  from 
trial  3  of  the  previous  section  where  the  motion  is  a  fixed  30  rev/min  about  the  body  “x”  axis,  0  rev/min 
about  the  body  “y”  axis  and  5  rev/min  about  the  body  “z”  axis  was  re-evaluated  with  only  a  change  to  the 
Ti  =  x2  parameter.  As  shown  in  Figure  1 1.17,  the  residual  for  the  estimate  of  0)b  when  using  the  optimized 
parameter,  clearly  violates  the  theoretical  boundary  established  from  the  covariance  of  the  residual. 
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residual  for  omega  1 


fame  (sec) 


Figure  11.17:  Example  of  Clear  Residual  Departure  Indicating  Improper  Filter  Performance 


Figure  1 1.18  illustrates  the  corresponding  filter  estimate  of  ©j.  If,  in  a  real  application,  this  was  observed, 
the  time  constants  and/or  process  noise  could  be  adjusted.  For  instance,  changing  and  t2  to  equal  le7 
versus  the  “optimized”  0.34  yields  approximately  the  same  attitude  determination  performance,  but 
radically  improves  the  estimation  of  the  rotational  rates  and  overall  figure  of  merit.  Figure  11.19 
illustrates  the  new  residual  for  g>i  which  now  satisfies  the  68  percent  criteria  relative  to  the  theoretical 
bounds  and  indicates  proper  filter  performance.  Figure  1 1.20  demonstrates  the  much  improved  estimation 
of  ©i  and  Table  11.7  summarizes  the  change  in  performance  associated  with  this  change  in  parameter. 
From  an  implementation  standpoint,  either  the  “optimized”  filter  parameters  could  be  set  to  more 
conservative  values  to  increase  filter  robustness,  with  a  corresponding  decrease  in  performance  under  ideal 
conditions,  or  the  algorithm  could  be  designed  to  observe  the  residual  and  make  changes  accordingly. 
Examples  where  this  method  appears  to  work,  like  the  two  illustrated  here,  may  certainly  be  found.  While 
this  method  holds  promise  as  a  “flag”  for  improper  filter  performance,  the  indicators  are  not  necessarily  as 
clear  as  those  observed  for  the  linear  Kalman  filter  case  demonstrated  in  [17],  and  further  research  is 
warranted  regarding  the  reliability  of  this  approach  for  the  UKF-based  filter. 
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Figure  11.20:  Filter  Estimation  of  caA  Following  Change  of  Time  Constant 


Table  11.7:  Summary  of  Performance  Before  and  After  Change  of  Time  Constant 
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11.7  A  Discussion  of  Error  in  the  Estimate 

Having  developed  the  algorithm  and  evaluated  its  performance,  it  is  important  to  identify  error  sources 
that  may  impact  the  accuracy  of  the  solution.  One  not  very  obvious  limitation  on  this  method  is  the 
accuracy  of  the  solar  ephemeris  and  magnetic  field  models  from  which  the  reference  vectors  are  computed 
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to  be  fed  into  the  Gauss-Newton  error  minimization  routine.  The  development  herein  assumes  the 
availability  of  these  reference  vectors  at  each  time  step.  While  the  solar  ephemeris  is  typically  very  well 
known,  the  magnetic  field  models  are  not  nearly  as  accurate  and  are  constantly  being  refined. 
Additionally,  while  the  Sun  is  a  relatively  constant  parameter,  the  magnetic  field  is  continuously  changing, 
primarily  due  to  solar  activity.  To  gain  maximum  accuracy  out  of  the  UKF-based  approach,  the  most 
accurate  model  values  possible  must  be  used.  To  pull  reference  vectors  from  either  model  requires  the 
location  of  a  point  of  interest.  Again,  in  this  development,  the  position  of  the  rocket  is  assumed  to  be 
known,  and  would  most  likely  come  from  either  radar  or  GPS  measurements  in  a  real-world  application. 
As  noted  in  Chapter  1,  great  progress  is  being  made  toward  reliable  implementation  of  GPS  positioning  on 
sounding  rockets,  and  this  will  help  minimize  the  errors  in  the  position  vector  that  would  be  used  to 
calculate  solar  and  magnetic  field  reference  vectors  from  the  appropriate  models.  Nevertheless,  some 
level  of  error  will  be  associated  with  the  modeled  values  and  this  will  translate  into  error  in  the  attitude 
solution.  This  error  is  not  unique  to  this  attitude  determination  method,  but  is  one  that  a  system  designer 
must  take  into  account.  Obviously  the  errors  in  the  sensor  measurements  contribute  to  the  overall  error  in 
the  solution,  and  Chapters  10  and  1 1  have  attempted  to  give  a  characterization  of  the  amount  of  error  that 
would  be  typical  for  a  given  sensor  accuracy.  Numerical  processing  errors  such  as  those  due  to  round  off, 
integration  step  size  and  method,  and  iteration  tolerance  contribute  to  the  overall  error.  As  discussed  in 
the  sections  on  algorithm  design  and  the  tuning  of  parameters,  attempts  have  been  made  to  minimize  these 
errors  while  taking  into  account  the  desire  to  produce  an  algorithm  that  can  be  implemented  in  real  time. 
The  accuracy  of  the  estimates  produced  is  also  impacted  by  how  close  the  actual  motion  is  to  the  motion 
prototype  used  to  “tune”  the  filter.  The  farther  the  true  motion  is  from  the  predicted  motion,  the  less 
accurate  the  estimate.  Finally,  as  noted  in  [39],  there  are  random  errors  at  each  stage  of  the  procedure. 
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12.0  Conclusions 

12.1  Significant  Conclusions 

The  principle  result  of  this  work  is  a  new  algorithm  for  the  attitude  determination  of  a  rotating  body  using 
low-cost  sensors.  This  algorithm  is  based  on  a  quaternion  formulation  to  avoid  the  well-known 
singularities  associated  with  Euler  angles  and  other  three-parameter  attitude  representations.  In  all,  four 
algorithms  were  designed,  programmed  and  tested.  Each  used  a  Gauss-Newton  error  minimization  to 
reduce  the  number  of  measurements  to  be  processed  by  the  filter  by  reducing  two  vector  measurements 
into  a  single  four-element  quaternion.  These  four  elements,  combined  with  three  body  rates,  yield  seven 
measurements  to  be  processed  by  the  filter  instead  of  the  original  nine.  The  sensors  assumed  for  this 
development  are  a  Sun  sensor,  providing  a  three-dimensional  Sun  vector  in  the  body  frame,  and  a  three- 
axis  magnetometer,  providing  noisy  measurements  of  the  magnetic  field  vector  in  the  body  frame.  Any 
two  vector  sensors  could  be  substituted  given  that  models  exist  to  determine  their  corresponding 
measurement  values  in  the  inertial  frame.  This  constraint  on  possible  sensors  is  necessary  to  meet  the 
requirements  for  the  solution  of  the  classical  Wahba’s  problem.  Two  extended  Kalman  filters  (EKF)  were 
developed  to  establish  a  performance  reference  based  on  a  “traditional”  approach  to  this  nonlinear 
problem.  One  filter  takes  rotational  rate  measurements  from  rate  sensors,  while  a  second  EKF  formulation 
relies  on  the  differencing  of  quaternions  to  provide  rate  “measurements”  to  the  filter.  Both  were 
demonstrated  to  successfully  estimate  attitude  motion  of  the  rotating  object,  albeit  with  a  performance 
penalty  when  using  differencing  in  place  of  true  measurements.  Breaking  from  the  traditional  approach, 
two  additional  algorithms  were  designed  and  implemented  based  on  the  recently  developed  unscented 
Kalman  filter  (UKF).  This  was  an  attempt  to  capitalize  on  the  superior  mean  and  covariance  propagation 
properties  of  the  UKF  relative  to  the  EKF,  to  achieve  better  performance  given  the  constraints  of  this 
problem.  It  is  the  first  known  application  of  the  UKF  to  an  attitude  determination  problem.  Again,  one 
approach  relied  on  differencing  of  quaternions  to  provide  rate  information,  while  the  second  UKF-based 
algorithm  accepted  rates  from  sensors.  The  UKF-based  algorithms  were  demonstrated  to  not  only 
successfully  estimate  attitude  motion,  but  to  outperform  the  EKF  algorithms.  In  one  case,  the  UKF 
algorithm  that  incorporates  rate  measurements  produced  a  39  percent  improvement  in  the  angular  error  of 
the  rocket  spin  axis  with  respect  to  the  EKF,  which  also  incorporated  rate  measurements  All  four  filters 
relied  on  a  simplified  embedded  dynamic  model,  and  the  algorithms  developed  here  should  be  successful 
in  any  attitude  determination  application  assuming  the  appropriate  measurements  are  available,  the  filter  is 
tuned  for  the  expected  motion,  and  sufficiently  small  time  steps  are  used.  The  application  of  interest  for 
this  work  was  sounding  rocket  attitude  determination,  and  the  filters  were  tested  against  simulated  rocket 
rotational  motion,  at  rotational  rates  about  the  principle  spin  axis  up  to  225  revolutions  per  minute.  For  the 
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same  baseline  example  for  which  results  were  described  above,  the  UKF  filter  achieved  a  performance 
improvement  on  the  order  of  47  percent,  when  comparing  an  overall  figure  of  merit,  relative  to  the  best 
EKF  performance.  This  figure  of  merit  is  a  measure  of  estimation  performance  for  both  attitude  and 
rotational  rates.  This  translates,  in  terms  of  attitude  determination,  into  a  mean  pointing  accuracy  of  the 
spin  vector  of  1.97  degrees  from  true  as  opposed  to  3.24  degrees,  an  improvement  in  spin  axis 
determination  of  approximately  39  percent.  In  contrast,  the  spin  vector  accuracy  achieved  using  only 
Gauss-Newton  error  minimization  was  on  the  order  of  3.28  degrees.  Similar  significant  improvements 
were  achieved  for  the  remaining  two  axes  and  the  rotational  rate  estimates  as  well.  The  performance  of 
the  new  UKF-based  algorithm  was  found  to  be  well-behaved  at  widely  varying  sensor  accuracies, 
measurement  intervals,  and  in  the  case  where  there  is  an  extended  loss  of  measurements.  This  robust 
performance  establishes  this  algorithm  as  a  viable  alternative  to  the  traditional  EKF  approach  and  provides 
an  algorithm  suitable  for  implementation  using  low  cost  sensors. 

12.2  Recommendations  for  Further  Research 

While  this  effort  demonstrated  the  feasibility  of  estimating  rotational  motion  of  a  highly  dynamic  vehicle, 
with  or  without  the  use  of  rate  measurements,  additional  work  may  be  envisioned  for  the  development  and 
implementation  of  this  UKF  filter  algorithm.  Despite  the  simulation  of  “representative”  motion 
undertaken  in  this  work,  true  rocket  motion  displays  many  idiosyncrasies  that  cannot  be  simulated  in  a 
straightforward  manner.  These  are  the  very  things  that  make  analytical  derivation  of  a  high  fidelity  model 
difficult.  As  such,  testing  of  the  algorithm  against  real  attitude  data  would  allow  a  definitive  analysis  of 
how  well  it  does  with  respect  to  the  uncertainties  of  real-world  motion.  The  difficulty,  of  course,  is  having 
a  “truth”  with  which  to  compare  in  order  to  judge  performance.  To  successfully  accomplish  such  a 
validation,  it  is  necessary  to  obtain  data  from  a  rocket  flight  with  a  highly  accurate  (expensive)  attitude 
determination  system  on  board,  and  measurements  compatible  with  the  algorithm  developed  here.  In  this 
way,  the  high-accuracy  solution  could  be  used  as  a  “truth”  against  which  the  algorithm  solution  could  be 
compared.  Obviously  this  data  set  will  not  be  easy  to  obtain,  but  is  certainly  worth  investigating  as  a  next 
developmental  and  validation  step  for  a  sole  source,  low-cost,  attitude  determination  system  based  on  the 
algorithm  developed  here. 

The  test  cases  simulated  here  all  assume  a  Gaussian  noise  profile.  Additional  work  is  warranted  to 
evaluate  how  this  UKF  algorithm  performs  when  the  noise  on  the  sensor  measurements  is  not  purely 
additive  white  Gaussian  noise  (AWGN),  but  has  a  “random  walk”  or  other  characteristic.  Some  insight 
into  this  problem,  with  respect  to  Kalman  filters  in  general,  is  given  by  Grewal  et  al.  who  suggest 
appending  a  new  variable  to  the  state  vector  and  modifying  the  0 ,  Qh  and  H  matrices  accordingly  [87].  A 
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similar  approach  should  be  investigated  for  the  UKF.  Since  the  UKF  has  a  parameter,  p,  that  is  optimized 
for  a  Gaussian  distribution  when  its  value  is  2  [68],  varying  this  value  may  make  the  UKF  better  suited  to 
other  statistical  distributions.  At  the  time  of  this  writing,  not  much  research  appears  to  have  been  done  in 
this  area. 

As  noted  early  on  in  this  work,  the  development  of  low  cost  sensors  is  ongoing.  One  intriguing 
development  is  the  recent  availability  of  low  cost  accelerometers.  These  provide  a  potential  additional 
source  of  rate  information  that  could  replace,  or  augment,  that  provided  by  the  rate  gyroscopes  assumed 
herein  for  the  baseline  sensor  suite.  Since  these  could  be  easily  included  in  a  rocket  sensor  suite,  the 
intelligent  incorporation  of  their  outputs  should  be  investigated. 

Further  research  is  also  warranted  into  characterizing  the  tuning  of  the  model  parameters.  While  an 
empirical  process  is  demonstrated  in  this  work,  where  simulation  data  is  available,  a  formal  process  for 
tuning  the  parameters  in  the  absence  of  such  data  would  be  helpful.  Fortunately,  the  accuracy  of  real 
sensor  measurements  may  be  estimated  pre-launch,  thereby  mitigating  some  of  the  uncertainty  in  the 
tuning  process.  This  leaves  the  overall  process  noise  and  the  time  constants  of  the  rotational  motion  to  be 
determined.  These  are  functions  of  the  predicted  rotational  motion,  the  sensor  characteristics,  and  the  lack 
of  fidelity  in  the  embedded  dynamic  model.  If  data  is  to  be  post-processed,  values  that  minimize  the  mean 
of  the  residual  over  the  duration  of  the  data  set  might  be  used.  Other  techniques  should  be  investigated  for 
use  in  a  real-time  implementation. 

Finally,  the  code,  as  implemented  here,  has  a  decidedly  developmental  aspect  to  it  with  many  “features” 
included  for  user-interaction,  data  analysis  and  graphical  display  of  values.  While  this  is  particularly  well- 
suited  for  use  as  a  developmental  test  bed  for  evaluating  a  wide  variety  of  scenarios,  for  implementation  in 
an  actual  application,  much  work  can  be  done  to  streamline  the  software  to  radically  decrease  execution 
times.  Despite  the  fact  that  MATLAB  is  an  exceptional  product  for  engineering  development,  much  faster 
alternative  languages  and  programming  structures  are  available  to  the  system  engineer  incorporating  this 
algorithm.  Such  a  streamlining  effort  would  go  a  long  way  toward  realizing  the  potential  of  this  algorithm 
as  a  real-time  process. 
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This  appendix  contains  the  software  written  to  implement  this  research  effort.  All  software  was  developed 
using  the  MATLAB  software  package.  It  is  written  with  analysis  and  design  of  the  filter  algorithms  in 
mind  and  contains  many  “extra”  features  that  are  not  necessary  when  implementing  any  one  of  the 
algorithms  for  an  actual  application.  One  example  of  this  is  the  extensive  use  of  plots  for  diagnostic 
purposes,  and  to  illustrate  operation  of  the  various  simulations  and  filters.  It  is  also  worthy  of  note  that 
MATLAB  is  an  exceptional  program  for  design  and  research,  and  has  many  built-in  functions  that  are  both 
extensive  and  user-friendly.  It  is  not,  however,  an  extremely  fast  programming  method.  Only  a  minimal 
effort  was  made  to  streamline  the  programming,  although  choices  were  made,  such  as  numerical 
integration  approach,  which  translate  into  time  savings  regardless  of  implementation  language.  The 
programming  style  is  certainly  not  elegant,  and  many  optimizations  can  be  made  to  speed  up  execution,  to 
include  programming  the  filter  of  choice  in  a  more  efficient  computer  language.  For  all  of  these  reasons, 
the  software  as  currently  written  serves  its  purpose,  but  is  slower  than  can  be  expected  in  an  actual 
application.  The  following  sections  describe  the  various  routines  used  for  this  effort. 

A.l  Attitude_filter.m 

Attitude_filter.m  is  the  main  MATLAB  script  for  this  software  implementation.  It  calls  the  other  software 
scripts  and  functions  defined  in  this  appendix  at  the  appropriate  time  to  simulate  the  “true”  motion  and  the 
noisy  measurements  of  the  motion.  Next  it  processes  the  noisy  measurements  through  the  Gauss-Newton 
error  minimization  routine  and  through  the  filtering  algorithm  selected  by  the  user.  This  is  the  “master” 
script  that  is  run  by  the  user. 


%Attitude_filter.m  —  main  script  to  call  m-files  to  execute  filtering  of  noisy  attitude  sensor  data. 

%  Implements  both  a  traditional  Extended  Kalman  Filter  and  an  "Unscented"  Kalman  Filter. 

%  Inclusion  of  rate  measurements  is  a  user-selectable  option. 

%  Written  by  Mark  Charlton.  Last  updated  21  October  03. 

format  compact 
clc; 

'This  program  calls  the  following  m-files  to  simulate  the  Kalman  filtering  of  noisy  attitude  sensor' 

'data  for  a  sounding  rocket  subject  to  rotational  motion.  Attitude_sim.m  simulates  the  rotational' 
'motion  as  well  as  the  noisy  measurements  of  that  motion.  A  traditional  Extended  Kalman  Filter  (EKF)’ 
'is  implemented  in  norates_ekf.m  and  in  rates_ekf.m  while  an  "Unscented"  Kalman  Filter  (UKF)  is’ 


'implemented  in  norates_ukf.m  and  in  rates_ukf.m.  Attitude_sim.m  generates  the  simulated  true  three-' 
'dimensional  motion  of  the  object,  as  well  as  simulated  attitude  sensor  measurements  in  the  form  of 
'3D  vectors.  It  also  generates  simulated  noisy  rate  measurements.  These  measurements  have  user-' 

'defined  sigmas  for  the  measurement  noise.  Norates_ekf.m  and  norates_ukf.m  filter  the  data  generated’ 

'by  attitude_sim.m  as  if  no  rate  measurements  were  available  and  plot  filter  results  vs.  truth  data.' 

'These  m-files  also  compute  the  Mean-Square-Error  (MSE)  for  each  technique  to  aid  in  judging  algorithm* 
'performance.  Rates_ekf.m  and  rates_ukf.m  do  similar  filtering  and  performance  measurement,  only  now' 

’they  include  the  simulated  noisy  rate  measurements.  From  attitude_filter.m,  the  user  may  choose  to* 

'generate  a  new  set  of  attitude_sim.m  generated  data  for  each  filter  run,  or  may  choose  to  continue' 

'filtering  the  same  data  while  changing  parameters  of  the  filter.  This  aids  in  investigating  both  how’ 

'a  given  set  of  filter  parameters  performs  on  a  set  of  data  as  well  as  how,  for  a  given  set  of  data,' 

'the  filter  performance  varies  with  changes  in  parameters.  Reasonable  Default  values  are  available' 

%for  each  parameter.' 

'press  ''return"  to  continue...' 
pause;clc;close; 

process=l; 
generated ; 

while  process— 1 
clc; 

'You  may  either  call  attitude_sim.m  to  generate  a  new  set  of  simulated  data  by’ 

'entering  "1",  or  process  the  existing  data  again  by  entering  "0”.' 

'If  this  is  the  first  run,  you  must  generate  data  (enter  "1").' 

generate=input(*Enter  "1"  or  ”0"  ("return"  defaults  to  same  data):  '); 
if  isempty(generate) 
generate=0 
end 

if  generate=l 
clear  all 

attitude_sim  %call  m-file  to  generate  truth  and  noisy  measurements 

generated ; 

clc 

<%,******  once  data  is  generated,  calculate  attitude  quaternions  from  noisy  measurements  ****** 
if  generate=l  %indicates  new  data  has  been  generated,  and  minimization  should  be  accomplished 


*******  Gauss-Newton  error  minimization  ******* 

'Press  "return"  to  calculate  the  attitude  quaternion  from  the  noisy' 
'attitude  measurements  using  a  Gauss-Newton  error  minimization...' 
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pause;close;clc; 

vref=[  ]; 
vrot=[  ]; 
q_min=[  J; 
q_min_dot=[  ]; 

qminlinit=0;  %initial  estimate  of  attitude  quaternion 

qmin2init=0; 

qmin3init=0; 

qmin4init=l; 


for  i=l  :count; 

vref(l  :6,i)=[SUNVEC(l  :3,i);MAGVEC(l  :3,i)] ; 


%form  reference  vector  from  known  inertial  vectors 


unitsunmeas(l  :3,i)=sunmeas(l  :3,i)/norm(sunmeas(l  :3,i));  %insure  noisy  sun  measurement  is  unit  vector 

unitmagmeas(l  :3,i)=magmeas(  1 :3,i)/norm(magmeas(  1 :3,i));  %insure  noisy  mag  measurement  is  unit  vector 

vrot(l  :6,i)=[unitsunmeas(l  :3,i);unitmagmeas(l  :3,i)];  %form  vector  to  be  rotated  by  "best"  quaternion 


if  i<2 

q_init=[qminlinit  qmin2init  qmin3init  qmin4init]‘; 
else 

qjnit=q_min(l  :4,i-l); 
end 


if  norm(sunmeas(l  :3,i))<.01  ^default  values  in  case  of  loss  of  data  (no  sun  vector  measurement) 

q_min(l:4,i)=[qminlinit  qmin2init  qmin3init  qmin4init]’; 

A(1 :4,1 :6,i)=A(l  :4, 1 :6,i-l); 
check(i)=check(i- 1 ); 
err(i)=err(i-l); 

elseif  norm(magmeas(l  :3,i))<.01  ^default  values  in  case  of  loss  of  data  (no  mag  field  measurement) 

q_min(l:4,i)=[qminlinit  qmin2init  qmin3init  qmin4init]'; 

A(1 :4tl  :6ti)=A(l:4,l  :6,i-l); 
check(i)=check(i-l ); 
err(i)=err(M); 
else 

%if  measurements  exist...proceed  with  minimization... 

steps=10;  %defines  max  allowable  iterations  for  convergence  within  Gauss„newton 

tol=.0 1 ;  %defines  convergence  tolerance  for  Gauss_Newton 

[q_min(l:4,i),  A(  1:4,1:64),  check(i),  err(i)]=Gauss„newton(vref(l:6,i),  vrot(l:6,i),  qjnit,  steps,  tol); 

%******  generate  noisy  measurements  of  quaternion  rate  by  differencing  subsequent  quaternions  ****** 
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%******  these  results  are  used  as  "derived"  rate  measurements  when  filtering  is  done  without  ****** 

%******  noisy  rate  measurements  from  sensors  ****** 

if  i>l  ^quaternion  differencing  to  get  qdot  and  then  omega  requires  one  time  step  delay!! ! 

q_min_dot(l  :4,i-l)=(q_min(l  :4,i)-q_min(l  :4,i-l))/TS; 

%******  calculate  body  rates  corresponding  to  derived  quaternion  and  quaternion  rates  ****** 

omega  l_q_min(i-l)=2*(q_min(4,i-l)*q_min„dot(l,i-l)+q_min(3,i-l)*q_min_dot(2)i-l)~q_min(2,i-l)*q_min_dot(3,i- 
1  )-q_min(  1  ,i- 1  )*q_min_dot(4,i- 1 )); 

omega2_q__min(i- 1  )=2*(-q_min(3,i- 1  )*q_min_dot(  l  ,i- 1  )+q_min(4,i-  l)*q_min_dot(2,i- 1  )+q_min(  l  ,i- 
1  )*q_min_dot(3,i- 1  )-q_min(2,i- 1  )*q_min__dot(4,i- 1 )); 

omega3_q_min(i-l  )=2*(q_min(2,i-  l)*qr_min„dot(  1  ,i- 1  )-q_min(  1  ,i- 1  )*q_min_dot(2,  i- 1  )+q_min(4,i- 1  )*q_min_dot(3,i- 
1  )-q_min(3,i- 1  )*q_min_dot(4,i- 1 )); 
end 
end 
end 

%******  Calculate  standard  deviation  of  error  in  measured  omegas  to  use  in  filter  R  matrices  —  not  available  in  real  world  ****** 
stdomega  t  =std(omega  1  ( I  :length(omega  l_q_min))-omega  l_q_min); 

stdomega2=std(omega2(l  :length(omega2_q_min))-omega2_q_min);  %use  these  when  no  rate  measurements  are  available 
stdomega3=std(omega3(l:length(omega3_q_min))-omega3_q_min); 

stdomega  1  meas=std(omega  1  measerror); 

stdomega2meas=std(omega2measerror);  %use  these  when  rate  measurements  are  available 

stdomega3meas=std(omega3measerror); 

%******  Plots  of  derived  quaternion  components  versus  true  quaternions  from  omega_sim  ****** 

plot(T(l:count),q_min(l,l:count),'g+-',  T(lxount),  Qvec(l,l:count)),grid  on; 
title('derived  ql  vs.  Euler-based  qr);xlabel('time  (sec),);ylabel(,ql,>; 
legend(’derived  ql’,  *  Euler-based  ql’); 
pause;close;ctc; 

plot(T(  1  xount),q_min(2, 1  :count),’g+-',  T(  1  ;count),  Qvec(2, 1  :count)),grid  on; 
title(’derived  q2  vs.  Euler-based  q2’);xlabel(’time  (secy^ylabelCqT); 
legend(’derived  q2\  'Euler-based  q2'); 
pause  ;close;clc; 

plot(T(lxount),q_min(3,lxount),'g+-\  T(l:count),  Qvec(3,lxount)),grid  on; 
title(’derived  q3  vs.  Euler-based  q3');xlabel('time  (sec)');ylabel(,q3'); 
legend(‘derived  q3',  'Euler-based  q3’>; 
pause;close;clc; 
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plot(T(l:count),q_min(4,l:count),'g+-',  T(I:count),  Qvec(4,l:count)),grid  on; 
title('derived  q4  vs.  Euler-based  q4');xlabel(,time  (sec)');ylabel('q4'); 
legend('derived  q4’,  'Euler-based  q4'); 
pause;close;clc; 

%******  rotate  each  vector  using  actual  and  derived  attitude  quaternion  ****** 

%******  check  mean-square-error  between  vectors  ****** 

*******  Evaluate  Mean-Square-Error  of  rotated  vectors  using  Gauss-Newton  derived  quaternion  ******* 
’Press  "return”  to  rotate  the  reference  vector  at  each  time  step  to  the’ 

’representation  in  the  rotated  frame  using  both  the  actual  rotation  quaternion’ 

'and  the  quaternion  derived  from  the  noisy  measurements.. .once  rotation  is  complete,' 

'calculate  the  mean-square-error  between  the  results.' 
pause;close;clc; 

GN_mse_error=[  ]; 


forj=l:count; 

Q_refvec(  1 :6,j)=qframerot6(Qvec(  1 :4  j),vref(  1 :6  j));  %rotate  ref  vector  using  true  quaternion 

q_minvec(l  :6 ,j)=qframerot6(q_min(l  :4  j),vref(  1 :6,j»;  %rotate  ref  vector  using  min  error  quaternion 

GN_mse_error(j  )=mean_square_error(q_minvec(  1 :6,j),Q_refvec(  1 :6,j)); 

%calculate  mse  between  rotated  vectors 


end 


'The  average  mse  error  for  the  reference  vectors  rotated  using  the  GN-derived  quaternion  is: 
mean_GN_mse_erron=mean(GN_mse_error) 

%******  Plot  vectors  rotated  using  derived  and  actual  quaternions  ***** 

plot3(Q_refvec(  1 , 1  :count),Q_refvec(2, 1  :count),Q_refvec(3, 1  :count),'b');grid  on;hold  on; 
plot3(q_minvec(  1 , 1  :count),q_min  vec(2, 1  :count),q_minvec(3, 1  :count),’m:'); 
plot3(Q  jrefvec(4, 1  :count),Q_refvec(5, 1  :count),Q_refvec(6, 1  :count),Y);grid  on;hold  on; 
p!ot3(q_minvec(4, 1  :count),q_minvec(5, 1  :count),q_minvec(6,  l  :count),’g:’); 
xlabel('X  component');yIabel('Y  component');zlabel('Z  component'); 

legend(’sun  vector,  actual  qVsun  vector,  derived  qVmag  field  vector,  actual  q',  'mag  field  vector,  derived  q'); 
title(’3-D  plot  of  reference  vectors  rotated  using  actual  quaternions  and  derived  quaternions’); 
pause;close;c!c; 

%******  Plot  mean-square-error  between  reference  vector  rotated  using  actual  quaternion  ****** 

%******  and  reference  vector  rotated  using  derived  quaternion.  ****** 


plot(T(  1  :count),GN_mse_error(  1  :count)),grid  on; 


titleCMSE  between  reference  vectors  rotated  using  derived  and  actual  quaternions'); 
xlabel(‘time  (sec),);ylabel(,MSE');legend(,MSE,) 

'press  "return"  to  continue...' 
pause;c!ose;clc; 
end 
end 

’Indicate  whether  to  use  rate  measurments  in  this  filtering  run... ' 

ratemeas=input('Enter  1  if  rate  measurements  are  available,  0  if  rates  are  derived  from  GN:  ') 
if  isempty(ratemeas) 
ratemeas=0 
end 

if  ratemeas=0 

*******  ^  mte  sensordata  is  being  used...  ******' 


*******  Extended  Kalman  Filter  ******* 


'Press  "return"  to  filter  data  from  attitude_sim.m  using  a  traditional  Extended  Kalman  Filter...’ 

'Press  return  to  advance  through  various  options...' 
pause;ctc;cIose; 

norates_ekf  %call  m-file  to  filter  data  with  traditional  extended  Kalman  filter 


*******  Unscented  Kalman  Filter  ******* 


'press  "return"  to  filter  data  from  attitude_sim.m  using  an  "Unscented"  Kalman  Filter...' 
pause;clc;cIose; 

norates_ukf  %call  m-file  to  filter  data  with  "unscented"  Kalman  filter 

'press  "return"  to  continue...' 
pause;clc;close; 

elseif  ratemeas==l 

*******  Rate  sensor  data  js  being  used...  ******' 


*******  Extended  Kalman  Filter  ******* 


'Press  "return"  to  filter  data  from  attitude„sim.m  using  a  traditional  Extended  Kalman  Filter...' 


’Press  return  to  advance  through  various  options...' 
pause;clc;close; 

rates_ekf  %call  m-file  to  filter  data  with  traditional  extended  Kalman  filter 

*******  Unscented  Kalman  Filter  ******* 

’press  "return"  to  filter  data  from  attitude_sim.m  using  an  "Unscented”  Kalman  Filter...' 
pause;clc;close; 

rates_ukf  %call  m-file  to  filter  data  with  "unscented"  Kalman  filter 

'press  "return"  to  continue...' 
pause;clc;close; 

end 

******  Recap  of  relative  performance  ****** 

'Indicator  of  overall  attitude  and  rotation  rate  performance  by  GN...' 
overalLGN„perf 

'Indicator  of  overall  attitude  and  rotation  rate  performance  by  EKF...’ 

I  I 

overall_ekf_perf 

’Indicator  of  overall  attitude  and  rotation  rate  performance  by  UKF...’ 

overall_ukf_perf 

’press  "return  to  continue..."' 
pause;close;c!c; 

************************************************************** **************** 

'***********  End  of  this  run — Do  you  wish  to  run  another  case?  ************** 

I****************************************************************************' 


process=input('Enter  "1"  for  yes  or  "0"  for  no.  Default  is  to  stop.  '); 

if  isempty  (process) 
process=0 
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end 

end 

'end  of  program' 

A.2  Attitude jsim.m 

Attitude_sim.m  is  called  by  attitude_filter.m  to  simulate  true  motion  of  the  vehicle  and  the  noisy  sensor 
measurements  of  that  motion.  Attitude_sim.m  produces  “true”  Euler  angles  and  “true”  attitude 
quaternions  at  each  time  step.  The  user  has  the  option  to  enter  the  desired  sensor  measurement  standard 
deviations,  the  initial  conditions,  the  simulation  duration,  and  the  type  of  rate  profile  desired,  among  other 
things. 


%Attitude_sim-  simulates  "true"  object  attitude  motion  by  propagating 
%Euler  angles  forward  through  the  numerical  integration  of  the  Euler  equations 
%using  a  second-order  Runge-Kutta  integration.  The  Euler  angular  rate  profile 
%is  calculated  from  a  user  defined  profile  of  the  angular  rates  in  the  body- 
%fixed  frame.  The  propagated  Euler  angles  are  used  at  each  time  step  to  form  a 
%direction  cosine  matrix  which  is  used  to  rotate  fixed  inertial  reference 
%vectors  into  the  current  body  frame.  Noisy  attitude  sensor  measurements  and  noisy 
%measurements  of  angular  rate  are  then  simulated  by  adding  random  noise  with  a  defined 
%sigma  corresponding  to  each  sensor.  The  corresponding  attitude  quaternion  is  computed 
%at  each  time  step  from  the  propagated  Euler  angles.  The  user  may  select  a  profile  with 
%fixed  angular  rates,  linearly  ramped  angular  rates,  or  rate  profiles  that  asymptotically 
^approach  a  final  value. 

%Written  by  Mark  Charlton,  last  modified:  28  October  2003 
^References:  [40],  [30],  [11] 

clear  all; 
clc; 

format  compact 
format  short 

*************  SIMULATION  OF  "TRUE"  OBJECT  ATTITUDE  MOTION  AND  NOISY  SENSOR  MEASUREMENTS 
************* 
t  I 

'NOTE:  All  attitude  vector  components  are  referenced  to  an  Earth-Centered  Inertial  reference’ 

'frame.  Therefore  positive  X  is  pointed  to  the  first  point  of  Aries,  positive  Z  is  out  the' 

'Earth  north  pole,  and  Y  completes  the  right-handed  orthogonal  triad.  When  asked  to  enter* 

'values,  hit  "return”  for  default  value.' 
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'NOTE:  This  uses  a  3-1-3  Euler  angle  sequence:  first  through  psi  about  the  original  Hz"  axis,' 
’then  through  theta  about  the  new  "x"  axis,  finally  through  phi  about  the  new  "z"  axis.' 


%************  input  measurement  errors  ************** 

’DATA  ENTRY  FOR  SIMULATION’ 

%enter  angular  standard  deviation  of  sun  sensor  measurement  in  degrees  (assumes  equal 
%measurement  error  in  all  axes) 

SIGSUN=input('PIease  enter  the  angular  standard  deviation  of  the  sun  sensor  measurement  in  degrees  (default  =  1.333):  '); 
if  isempty(SIGSUN) 

SIGSUN=1.333 

end 

%convert  sun  sensor  sigma  to  radians 
SIGSUN=S  IGSUN*pi/ 1 80; 

^convert  sun  sensor  sigma  from  angular  to  absolute  measure  for  unit  sun 
%  vector 

SIGSUN=sin(SIGSUN);  %based  on  unit  vector  and  small  angle  approximation 

%*****  for  now.. .SIGMA  is  same  in  each  axis  ***** 

SIGSUNX=SIGSUN; 

SIGSUNY=SIGSUN; 

SIGSUNZ=S  IGSUN ; 

Center  angular  standard  deviation  of  magnetometer  measurement  in  degrees  (assumes  equal 
^measurement  error  in  all  axes) 

SIGMAG=input('PIease  enter  the  angular  standard  deviation  of  the  magnetometer  measurement  in  degrees  (default  =  3.333):  ’); 
if  isempty(SIGMAG) 

S IGM AG=3 .333  %based  on  unit  vector  and  small  angle  approximation 

end 

%convert  magnetometer  sensor  sigma  to  radians 
SIGMAG=SIGMAG*pi/180; 

%convert  magnetometer  sensor  sigma  from  angular  to  absolute  measure 
%for  unit  sun  vector 

SIGMAG=sin(SIGMAG); 


%*****  for  now...SIGMA  is  same  in  each  axis  ***** 
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SIGMAGX=SIGMAG; 

SIGMAGY=SIGMAG; 

SIGMAGZ=SIGMAG; 

%enter  standard  deviation  of  omega  sensor  measurement  in  rpm  (assumes  equal 
^measurement  error  in  all  axes) 

SIGOMEGA=input(’Please  enter  the  standard  deviation  of  the  rotational  rate  sensor  measurement  in  rpm  (default  =  0.333):  ’); 
if  isempty(SIGOMEGA) 

SIGOMEGA^O.333 

end 

%convert  omega  sensor  sigma  to  radians/sec 
SIGOMEGA=SIGOMEGA*2*pi/60; 


SIGOMEG  A 1  =S  IGOMEG  A; 
SIGOMEGA2=S  IGOMEG  A; 
SIGOMEGA3=S  IGOMEG  A; 


%************  jnpUt  reference  vectors  ************** 

%Initial  reference  sun  vector  X  component  (from  ephemeris  given  time  and 
%location) 

SUNX=input('Plea$e  enter  the  initial  true  X  component  of  the  sun  vector  (default  =  1)):  '); 
if  isempty(SUNX) 

SUNX=1 

end 

%  Initial  reference  sun  vector  Y  component  (from  ephemeris  given  time  and 
%location) 

SUNY=dnput(*PIease  enter  the  initial  true  Y  component  of  the  sun  vector  (default  =1):  '); 
if  isempty(SUNY) 

SUNY=1 

end 

%  Initial  reference  sun  vector  Z  component  (from  ephemeris  given  time  and 
Relocation) 

SUNZ=input('Please  enter  the  initial  true  Z  component  of  the  sun  vector  (default  =1):  '); 
if  isempty(SUNZ) 

SUNZ=1 


end 


^Initial  reference  mag  field  vector  X  component  (from  mag  field  model  given  location) 


MAGX=input('Please  enter  the  initial  true  X  component  of  the  unit  mag  field  vector  (default  =  -1):  ’); 
if  isempty(MAGX) 

MAGX=-1 

end 

%Initial  reference  mag  field  vector  Y  component  (from  mag  field  model  given  location) 

MAGY=input('Please  enter  the  initial  true  Y  component  of  the  unit  mag  field  vector  (default  =  1):  '); 
if  isempty(MAGY) 

MAGY=1 

end 

%Initial  reference  mag  field  vector  Z  component  (from  mag  field  model  given  location) 

MAGZ=input('Please  enter  the  initial  true  Z  component  of  the  unit  mag  field  vector  (default  =  -1):  ’); 
if  isempty(MAGZ) 

MAGZ=-t 

end 

omega  1=[  ]; 
omega2=[  ]; 
omega3=[  3; 
psi=[  ]; 

theta=[  ];  %initia!ization  of  matrices 

phi=[]; 
psidot=[  ]; 
thetadot=[  ]; 
phidot=[  ]; 

T=[]; 

%************  ^npUt  Euier  angles  ************** 

%  Input  initial  Euler  angle,  psi 

psiO=input('Please  enter  the  initial  value  of  the  Euler  angle  "psi"  in  degrees  (default  =  15):  '); 
if  isempty(psiO) 
psi0=15 
end 

%convert  psiO  to  radians 
psi0=psi0*pi/180; 


psi(l)=psiO; 


%Input  initial  Euler  angle,  theta 

thetaO=input('Please  enter  the  initial  value  of  the  Euler  angle  "theta"  in  degrees  (default  =  30):  '); 
if  isempty(thetaO) 
theta0=30 
end 

^convert  thetaO  to  radians 
theta0=theta0*pi/180; 

theta(l)=thetaO; 

%  Input  initial  Euler  angle,  phi 

phiO=input('Please  enter  the  initial  value  of  the  Euler  angle  "phi"  in  degrees  (default  =  45):  ’); 
if  isempty(phiO) 
phiO=45 
end 

%convert  phiO  to  radians 
phi0=phi0*pi/180; 

phi(l)=phi0; 


%************  input  total  simulation  time  ************** 


total_time=input(‘PIease  enter  the  desired  length  of  simulation  in  seconds  (default  =  40):  '); 
if  isempty(total„time) 
total_time=40 
end 

%******  determine  desired  omega  profile  and  call  appropriate  omega  generator  ******* 

'The  following  generation  methods  are  available:  1  =  fixed  body  rates,  2  =  linearly  ramped  body  rates,' 
’3  =  exponentially  derived  body  rates,  4  =  step  function  /  pulse' 

■  i 

method=input('Please  enter  the  number  of  the  desired  generation  method:  ') 

if  isempty(method) 
method=3 
end 


if  method  =  1 


%******  call  omegagen_fix  to  generate  arrays  of  angular  rates  about  the  rotated  frame  axes  ****** 


[omega  1,  omega2,  omega3,TS]=omegagen_fix(total_time); 
end 

if  method  =  2 

%******  call  omegagen_ramp  to  generate  arrays  of  angular  rates  about  the  rotated  frame  axes  ****** 

[omega  1,  omega2,  omega3,TS]=omegagen_ramp(total_time); 
end 

if  method  =  3 

%******  call  omegagen_exp  to  generate  arrays  of  angular  rates  about  the  rotated  frame  axes  ****** 

[omega  1,  omega2,  omega3,TS]=omegagen__exp(totaLtime); 

end 

if  method  =  4 

%******  call  omegagen_step  to  generate  arrays  of  angular  rates  about  the  rotated  frame  axes  ****** 

[omega  1,  omega2,  omega3,TS]=ornegagen_step(totaLtime); 
end 

%******  Numerical  integration  of  Euler  equations  ****** 

%******  Propagates  Euler  angles  forward  using  body  rates  and  initial  cond  ****** 

count=l ;  %initializes  counter 

H=TS/10;  %set  integration  time  step 

T=[]; 

T(1)=0;  %array  of  times  of  each  measurement 

%******  Calculate  initial  Euler  angle  derivatives  ****** 
if  abs(theta)<eps 

theta=le-4;  %prevent  divide  by  zero  /  singularity 

end 

if  abs(theta(count)-pi)<eps 
if  sign(theta(count)-pi)~l 
theta(count)=theta(count)+ le-4; 


else 


theta(count)=theta(count)- 1  e-4; 
end 
end 

psidot(count)=l/sin(theta(count))*(sin(phi(count))*omegal(count)+cos(phi(count))*omega2(count)); 

thetadot(count)=l/sin(theta(count))*(cos(phi(count))*sin(theta(count))*omegal(count)- 

sin(phi(count))*sin(theta(count))*omega2(count)); 

phidot(count)=l/sin(theta(count))*(-sin(phi(count))*cos(theta(count))*omegal(count)- 

cos(phi(count))*cos(theta(count))*omega2(count)+sin(theta(count))*omega3(count)); 


SUNVEC=[  ]; 
MAGVEC=[  ]; 
sunvec=[  ]; 
magvec=[  ]; 

ql=[]i 
q2=[]; 
q3=[  j; 
q4=[3; 

Qvec=[  ]; 
sunmeas=[  ]; 
magmeas=[  ]; 
sunmeaserror=[  ]; 
magmeaserror=[  ]; 


while  T(count)<=total„time 


%************  simulate  attitude  motion  in  terms  of  Euler  angles  ************** 


%****************************************************************** 


%*******  Calculate  Euler  angles  and  noisy  measurements  at  each  time  step  ******** 

%*******  Use  Euler  angles  to  generate  direction  cosine  matrix  at  each  time  ******* 
%*******  step  matrix  to  convert  from  inertial  to  body  axes.. .from  [30],  ******* 

% *******  pg  36  or  [40],  pg  7  ******* 

DCM(1 , 1  )=cos(phi(count))*cos(psi(count))-sin(phi(count))*cos(theta(count»*sin(psi(count»; 
DCM(l,2)=cos(phi(count))*sin(psi(count))+sin(phi(count))*cos(theta(count))*cos(psi(count)); 
DCM(l,3)=sin(phi(count))*sin(theta(count)); 

DCM(2,l)=-sin(phi(count))*cos(psi(count))-cos(phi(count))*cos(theta(count))*sin(psi(count)); 

DCM(2,2)=-sin(phi(count))*sin(psi(count))+cos(phi(count))*cos(theta(count))*cos(psi(count)); 

DCM(2,3)=cos(phi(count))*sin(theta(count)); 

DCM(3, 1  )=sin(theta(count))*sin(psi(count)); 

DCM(3,2)=-sin(theta(count))*cos(psi(count)); 

DCM(3,3)=cos(theta(count)); 
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%*******  Use  DCM  to  rotate  inertial  reference  vectors  to  body  frame  ******* 

%*******  Ref  vectors  are  hard-coded  or  user  entered  here,  but  would  ******* 

%*******  come  from  solar  ephemeris  and  mag  field  model  in  real  life  ******* 

%*******  given  time  and  position  from  radar,  gps  or  other  source  ******* 

SUNVEC(1 :3,count)=[SUNX;SUNY;SUNZ];  %builds  array  of  inertial  sun  reference  vector 

if  norm(SUNVEC)>eps 

SUNVEC(1 :3,count)=SUNVEC(l  :3,count)/norm(SUNVEC(l  :3, count));  ^insures  reference  vectors  are  unit  vectors 
end 

MAGVEC(1 :3,count)-[MAGX;MAGY;MAGZ];  %builds  array  of  inertial  mag  field  reference  vectors 

if  norm(MAGVEC)>eps 

MAGVEC(  1 :3,count)=MAGVEC(  l  :3,count)/norm(M AG VEC(  1 :3,count));  ^insures  reference  vectors  are  unit  vectors 
end 

sunvec(  1 :3,count)=DCM*SUNVEC(  1 :3, count);  %rotates  SUNVEC  to  body  frame 

magvec(  1  ;3,count)=DCM*MAGVEC(  1 :3,count);  %rotates  M AGVEC  to  body  frame 

%*******  Add  random  noise  with  defined  sigma  to  simulate  noisy  measurements  ****** 

sunmeas(  1  ,count)=sunvec(l  ,count)+SIGSUNX*randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 
sunmeas(2,count)=sunvec(2,count)+SIGSUNY*randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 
sunmeas(3,count)=sunvec(3,count)+SIGSUNZ*randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 
magmeas(  1  ,count)=magvec(  1  ,count)+SIGMAGX*randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 
magmeas(2,count)=magvec(2,count)+SIGMAGY*randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 
magmeas(3,count)=magvec(3,count)+SIGMAGZ*randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 

%*******  Add  random  noise  with  defined  sigma  to  simulate  noisy  measurements  ****** 

omega  1  meas(  1  ,count)=omega  1  (1  ,count)+S!GOMEGAl  *randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 
omega2meas(  1  ,count)=omega2(l  ,count)+S IGOMEGA2*randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 
omega3meas(l ,count)=omega3( l,count)+SIGOMEGA3*randn;  %builds  array  of  simulated  noisy  sun  sensor  measurements 

%*******  Convert  Euler  angles  to  the  "true”  attitude  quaternion  at  each  time  step  ******* 

%*******  This  provides  "truth”  to  compare  back  to  at  outcome  of  filtering  algorithm  ***** 

ql(count)=cos(psi(count)/2)*sin(theta(count)/2)*cos(phi(count)/2)+sin(psi(count)/2)*sin(theta(count)/2)*sin(phi(count)/2); 

q2(count)=-cos(psi(count)/2)*sin(theta(count)/2)*sin(phi(count)/2)+sin(psi(count)/2)*sin(theta(count)/2)*cos(phi(count)/2); 

q3(count)=cos(psi(count)/2)*cos(theta(count)/2)*sin(phi(count)/2)+sin(psi(count)/2)*cos(theta(count)/2)*cos(phi(count)/2); 

q4(count)=cos(psi(count)/2)*cos(theta(count)/2)*cos(phi(count)/2)-sin(psi(count)/2)*cos(theta(count)/2)*sin(phi(count)/2); 

Qvec(  1 :4,count)=[q  1  (count);  q2(count);  q3(count);  q4(count)] ;  %  array  of  rotation  quaternions 


^****iK****jt:j(c*Hc***********  Error  arrays  for  analysis  ********************* 
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sunmeaserror(  1 :3,count)=sunvec(l  :3,count)-sunmeas(  1 :3, count); 
magmeaserror(l  :3,count)=magvec(l  :3,count)-magmeas(l  :3, count); 
omega  1  measerror(l  :count)=omega  1(  1  :count)-omega  1  meas(l  :count); 
omega2measerror(l  :count)=omega2(l  :count)-omega2meas(l  :count); 
omega3measerror(l  :count)=omega3(l  :count)-omega3meas(l  :count); 


TI=0.; 

PSI=psi(count); 
THETA=theta(count); 
PHI=phi(count); 
OMEGAl^omegal  (count); 
OMEGA2=omega2(count); 
OMEG  A3=omega3  (cou  nt) ; 


^initialize  psi  for  numerical  integration 
^initialize  theta  for  numerical  integration 
^initialize  phi  for  numerical  integration 

^initialize  omega  1  for  numerical  integration 
^initialize  omega2  for  numerical  integration 
^initialize  omega3  for  numerical  integration 


^initialize  integration  timer 


if  abs(THETA)<eps 
THETA=le-4; 
id 

if  abs(THETA-pi)<eps 
if  sign(THETA-pi)=l 
THETA=theta+ 1  e-4; 
else 

THETA=theta-  le-4; 
end 
end 

if  abs(THETA-2*pi)<eps 
if  sign(THETA-2*pi)=l 
THETA=theta+ 1  e-4; 
else 

THETA=theta~  le-4; 
end 
end 

while  TI<=(TS-.0001) 


^prevent  divide  by  zero  /  singularity  at  theta=0  rad 
%prevent  divide  by  zero  /  singularity  at  theta=pi  rad 


^prevent  divide  by  zero  /  singularity  at  theta=pi  rad 


%*******  propagate  Euler  angles  forward  one  time  step  using  2nd  order  Runge-Kutta  ******* 
%*******  ajg0rithm  to  numerically  integrate  the  Euler  equations  ******* 


PSIOLD=PSI; 

THETAOLD=THETA; 

PHIOLD=PHI; 


PSIDOT=l/sin(THETA)*(sin(PHI)*OMEGAl+cos(PHI)*OMEGA2); 

THETADOT=l/sin(THETA)*(cos(PHI)*sin(THETA)*OMEGAl-sin(PH0*sin(THETA)*OMEGA2); 

PHIDOT=l/sin(THETA)*(-sin(PHI)*cos(THETA)*OMEGAl-cos(PHI)*cos(THETA)*OMEGA2+sin(THETA)*OMEGA3); 
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PS  I=PS  I+H  *PS  IDOT ; 

THETA=THETA+H*THETADOT; 

PHI=PHI+H*PHIDOT; 


%calculate  psi  at  next  time  step 
^calculate  theta  at  next  time  step 
%calculate  phi  at  next  time  step 


PSrDOT=l/sin(THETA)*(sin(PHI)*OMEGAl+cos(PHI)*OMEGA2); 

THETADOT=l/sin(THETA)*(cos(PHI)*sin(THETA)*OMEGAl-sin(PHI)*sin(THETA)*OMEGA2); 

PHDDOT=l/sin(THETA)*(-sin(PHI)*cos(THETA)*OMEGAl‘COs(PHI)*cos(THETA)*OMEGA2+sin(THETA)*OMEGA3); 

PSI=.5*(PSI0LD+PSI+H*PSID0T);  %num  integrate  psi  forward 

THETA=  5*(THETAOLD+THETA+H*THETADOT);  %num  integrate  theta  forward 

PHI=.5  *(PHIOLD+PHI+H*PHIDOT);  %num  integrate  phi  forward 


TI=TI+H;  ^increment  integration  time 

end 


count=count+ 1 ;  ^increment  counter 

T(l,count)=T(l, count-  1)+TS;  ^increment  simulation  time 


%******  stone  quantities  of  interest  in  arrays  ****** 

psidot(count)=PSIDOT; 
thetadot(count)=THET  ADOT; 
phidot(count)=PHIDOT; 
psi(count)=PSI; 
theta(count)=THET  A; 
phi(count)=PHI; 

end 

%********  represent  psi,  theta,  phi  as  angles  between  0  and  2pi  ******* 

forj=l:count 
psi(j)=mod(psi(j),2*pi); 
theta(j)=mod(theta(j),2*pi); 
phi(j)=mod(phi(j),2*pi); 
end 

%********  Adjust  count  to  match  array  sizes  for  error  calculation  and  plotting  ****** 
%***  ...necessary  because  count  gets  incremented  before  failing  test  to  continue  loop  *** 


%represent  psi  between  0  and  2pi 
%represent  theta  between  0  and  2pi 
^represent  phi  between  0  and  2pi 


count=count-l; 
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%********  truncate  data  arrays  to  correct  length********** 

%***  ...necessary  because  T  gets  incremented  before  failing  test  to  continue  loop  *** 

T=T(l:count); 

%*******  Option  to  simulate  "drop-outs”  in  measurements  ******* 

’Please  press  "return”  to  continue...' 
pause;close;clc; 

'Simulation  data  generation  (continued)...' 

I  9 

'Do  you  wish  to  simulate  data  dropouts?  (default  is  no)' 

dropout=input('Please  enter  "1"  to  simulate  dropouts  or  "0"  for  no  dropouts.. .(default  is  no  dropouts):  '); 
if  isempty(dropout) 
dropout=0 
end 

if  dropout  =  1 

dropsunmeas=input('enter  duration  of  sunsensor  dropout  in  seconds:  ’); 
dropsunmeas=floor(dropsunmeas/TS); 

dropsunwhere=inputCenter  where  in  simulated  data  drop  occurs  as  a  decimal  (i.e.,  half  way  =  .5):  ’); 

dropmagmeas=input('enter  duration  of  magsensor  dropout  in  seconds:  '); 
dropmagmeas=floor(dropmagmeas/TS); 

dropmagwhere=input('enter  where  in  simulated  data  drop  occurs  as  a  decimal  (i.e.,  half  way  =  .5):  '); 

sunstartcount=floor(dropsunwhere*length(sunmeas)); 

sunmeas(l:3,sunstartcount:sunstartcount+dropsunmeas)=eps; 

magstartcount=floor(dropmagwhere*length(magmeas)); 
magmeas(  1 :3,magstartcount:magstartcount+dropmagmeas)=eps; 

t  f 

dropomegameas=input('enter  duration  of  rate  sensor  dropout  in  seconds:  ’); 
dropomegameas=floor(dropomegameas/TS); 

dropomegawhere=input('enter  where  in  simulated  data  drop  occurs  as  a  decimal  (i.e.,  half  way  =  .5):  ’); 
« > 

omegastartcount=floor(dropomega  where*Iength(omega  1  meas)); 
omega  1  meas(l  ,omegastartcount:omegastartcount+dropomegameas)=eps; 
omega2meas(l,omegastartcount:omegastartcount+dropomegameas)=eps; 
omega3meas(l,omegastaitcount:omegastartcount+dropomegameas)=eps; 
end 


^0^*t********************  Data  Review  and  Plotting  ********************** 


plots=[  ]; 

plots=input(’Enter  "1"  to  show  plots,  "0"  to  skip  plots.  Default  is  to  show  plots:  '); 
if  isempty(plots) 
plots=l; 
end 


if  plots  ==  1 

%****************  gun  vector  plots  ********************* 

plot(T,sunmeas(l,I:count),'g+',  T,  sunvec(l,l:count),  VtT,SUNVEC(l,l:countVo'),grid  on; 
titleCX  component  of  unit  sun  vector  vs.  time');xlabel('time  (sec),);yIabel('X  component'); 
legend('measurement,,  'rotated',  'inertial'); 
pause;close;clc; 

plot(T,sunmeas(2, 1  :count),'g+\  T,  sunvec(2,l:count),  ,r,,T,SUNVEC(2,l:count),'o'),grid  on; 
title('Y  component  of  unit  sun  vector  vs.  time');xlabel('time  (sec)');ylabel('Y  component'); 
legend('measurement',  'rotated',  'inertial'); 
pause;close;clc; 

plot(T,sunmeas(3,l:count),'g+',  T,  sunvec(3,l:count),Y,T,SUNVEC(3,l:count),'o'),grid  on; 
title(’Z  component  of  unit  sun  vector  vs.  time');xlabel(’time  (sec)');ylabel(’Z  component’); 
legend('measurement',  'rotated',  'inertial'); 
pause;c!ose;clc; 

plot3(sunvec(  1 , 1  :count),sunvec(2, 1  :count),sunvec(3,  l  :count),'r+');grid  on;hold  on; 
plot3(SUNVEC(  1 , 1  :count),SUNVEC(2, 1  :count),SUNVEC(3, 1  rcount),!)*'); 
axis([min(sun  vec(  1 , 1  :count))- 1  ,max(sun  vec(  1 , 1  :count))+l  ,min(sun  vec(2, 1  :count))- 
l,max(sunvec(2,l:count))+l,min(sunvec(3,l:count))-l,max(sunvec(3,l:count))+l]); 
xlabel('X  component');ylabel('Y  component');zlabel('Z  component’); 
legend('rotated’,'inertiar); 

title('3-D  plot  of  both  inertial  and  rotated  sun  vector  from  start  to  end  of  simulation'); 
pause;close;clc; 

^****************  mag  fieid  vector  plots  ********************* 

plot(T,magmeas(l,l:count),'g+',  T,  magvec(l,l:count)T  'r',T,MAGVEC(l,l  :count),'o');grid  on; 
titleCX  component  of  unit  mag  field  vector  vs.  time');xlabelCtime  (sec)');ylabel('X  component'); 
legend('measurement\  'rotated',  'inertial'); 
pause;close;clc; 


plot(T,magmeas(2,l:count),’g-f-',  T,  magvec(2,l:count),  ’r\T,MAGVEC(2,l:count),'o');grid  on; 
title('Y  component  of  unit  mag  field  vector  vs.  time');xlabel('time  (sec)');yIabel('Y  component'); 
legend('measurement',  'rotated*,  'inertial'); 
pause;close;clc; 

pIot(T,magmeas(3,l:count),'g+',  T,  magvec(3,l:count),  'r’,T,MAGVEC(3,l:count),'o,);grid  on; 
title('Z  component  of  unit  mag  field  vector  vs.  time*);xlabel('time  (sec)');ylabel('Z  component’); 
legendCmeasurement’,  'rotated',  'inertial'); 
pause;close;clc; 

plot3(magvec(  1 ,  l  :count),magvec(2, 1  :count),magvec(3, 1  :count),'m+’);grid  on;hold  on; 
plot3(M  AGVEC(  1 , 1  :count),MAGVEC(2, 1  :count),MAGVEC(3, 1  :count),*b*'); 
axis([min(magvec(  1 , 1  :count))- 1  ,max(magvec(  l ,  1  :count))+l  ,min(magvec(2, 1  xount))- 
l  ,max(magvec(2, 1  :count))+l  ,min(magvec(3, 1  :count))- 1  ,max(magvec(3,l  :count))+l]); 
xlabel('X  component');ylabeI('Y  component');zlabel(’Z  component'); 
legend('rotatedVinertiar); 

title('3-D  plot  of  both  inertial  and  rotated  mag  field  vector  from  start  to  end  of  simulation'); 
pause;close;clc; 

%******  plot  inertial  and  rotated  frame  sun  and  mag  field  vectors  ****** 

plot3(sun  vec(  1 , 1  :count),sunvec(2, 1  :count),sunvec(3, 1  :count),'r+');grid  on;hold  on; 

plot3(SUN  VEC(1 , 1  :count),SUNVEC(2, 1  :count),SUNVEC(3, 1  :count)/bd'); 

p!ot3(magvec(  1 , 1  :count),magvec(2, 1  :count),magvec(3, 1  :count),'mx'); 

plot3(M  AGVEC(  1 , 1  :count),MAGVEC(2, 1  :count),M  AGVEC(3, 1  :count),'ks'); 

plot3(q  1  ( 1  :count),q2(  1  :count),q3(  1  :count),'go'); 

xlabel('X  component');ylabel('Y  component');zlabelCZ  component'); 

legend('rotated  sun’, 'inertial  sun’, 'rotated  mag'/inertial  magVquatemion'); 

titleC3-D  plot  of  inertial  and  rotated  sun  and  mag  field  vectors  and  rotation  quaternion'); 

pause;close;clc; 


%****************  b0(jy  frame  omega  plots  ********************* 

plot(T,omegalmeas(l,l:count)*60/(2*pi),’g+',  T,  omega l(l,l:count)*60/(2*pi),  'r');grid  on; 
title('rotational  rate  about  body  axis  1  vs.  time');xlabelCtime  (sec)’);ylabel('omega_l  (rpm)'); 
legendCmeasurement',  'actual'); 
pause;close;clc; 

plot(T,omega2meas(l,l:count)*60/(2*pi),'g+',  T,  omega2(l,l:count)*60/(2*pi),  *r’);grid  on; 
titlefrotational  rate  about  body  axis  2  vs.  time’);xlabel('time  (sec)');ylabel('omega_2  (rpm)’); 
legendCmeasurement',  'actual'); 
pause;close;dc; 


plot(T,omega3meas(l,l:count)*60/(2*pi),'g+',  T,  omega3(l,l:count)*60/(2*pi),  'r^grid  on; 


title('rotational  rate  about  body  axis  3  vs.  time’^xlabelftime  (sec)');ylabel('omega_3  (rpm)’); 

legend('measurement\  'actual'); 

pause;cIose;clc; 

%******  plot  Euler  angle  rates  ****** 

psidotrpm=psidot*60/(2*pi);  %convert  angular  rates  from  rad/s  to  rev/min  for  plotting 

thetadotrpm=thetadot*60/(2*pi); 

phidotrpm=phidot*60/(2*pi); 

plot  (T(5:count),psidotrpm(5:count),,bo-‘,T(5:count),thetadotrpm(5:count),,rx-’,T(5;count),phidotrpm(5:count),'m 

grid  on;titleCEuler  angle  rates  versus  time  (without  initial  transients)'); 

xlabel('time  ($ec)’);ylabelOEuler  angle  rates  (rev/min)');legend('psidot\  'thetadot',  'phidot'); 

pause;close; 

t y0 ****************  Euler  angle  plots  ********************* 

degpsi=psi*  180/pi;  ^convert  psi  to  degrees  for  plotting 

degtheta=theta*  1 80/pi ;  %con vet  theta  to  degrees  for  plotting 

degphi=phi*  1 80/pi;  ^convert  phi  to  degrees  for  plotting 


plot(T,degpsi(l  :count),*mx'tT,degtheta(l  :count),*b+\T,degphi(l  :count),'r-'),grid  on; 
title(’Euler  angles  vs.  time');xlabel(’time  (sec)1);ylabel('Euler  angles’); 
legendCpsi  (deg)Vtheta  (deg)Vphi  (deg)'); 
pause;close;clc; 

% ****************  Attitude  quaternion  component  plots  ********************* 

plot(T,q4(l  :count),'mo\T,ql(l  :count),'bx,JT,q2(l  :count),*r+\T,q3(l  :count),'g*'),grid  on; 
title('attitude  quaternion  components  vs.  time');xlabel(’time  (sec)');ylabel('quatemion  components'); 
legend(  'q4\  'ql',  'q2\  *q3'); 
pause;close;c!c; 
end 

%******  Diagnostics  ****** 
diagnostics=[  ]; 

diagnostics=input('Enter  "1"  to  display  diagnostic  values,  "0”  to  skip  diagnostics.  Default  is  no  diagnostics:  '); 
if  isempty(diagnostics) 
diagnostics=0; 
end 


if  diagnostics  —  1 


q_rot=Qvec(l  :4, count); 


Qvecl=q_rot(l); 

Qvec2=q_rot(2); 

Qvec3=q_rot(3); 

Qvec4=q_rot(4); 

[axis,angle]=qrot(q_rot) 

'press  "return"  to  continue...' 
pause;close;clc; 

eul(  l)=psi(count)*  1 80/pi; 
euI(2)=theta(count>*  1 80/pi; 
eul(3)=phi(count)*  1 80/pi; 

The  final  Euler  angles  are:’ 

'psi=  *,eul(l) 

'theta=  ’,eul(2) 

’phi=  \eul(3) 

'The  final  rotated  vectors  are: ' 

'sunvec=  \sun=sunvec(l:3,count) 

'magvec-  \mag=magvec(l:3,count) 

a(  1 , 1  )=Q  vec  1  A2-Qvec2A2-Qvec3  A2+Q  vec4A2; 
a(l,2)=2*(Qvecl*Qvec2+Qvec3*Qvec4); 
a(  1 ,3)=2*(Qvec  1  *Qvec2-Q  vec3  *Qvec4); 
a(2,2)=:-QveclA2+Qvec2A2-Qvec3A2+Qvec4A2; 
a(2,3)=2*(Qvecl  *Qvec3+Qvec2*Qvec4); 
a(3,2)=2*(-Qvecl*Qvec4+Qvec2*Qvec3); 
a(3,3)=-Qvec  1  A2-Qvec2A2+Qvec3A2+Qvec4A2; 

vinit=[SUNVEC(  1 :3,count);M  AGVEC(  1 :3, count)] ; 
vfinal=[sunvec(l  :3,count);magvec(l  :3,count)]; 

SUN=SUNVEC(  1:3,1); 

MAG=MAGVEC(  1:3,1); 

'press  "return"  to  continue...' 
pause;c!ose;clc; 

'The  standard  dev  of  the  generated  sun  vector  x  components  is:',std(sunmeaserror(l,l:count)) 
'The  standard  dev  of  the  generated  sun  vector  y  components  is:’,std(sunmeaserror(2,l:count)) 
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The  standard  dev  of  the  generated  sun  vector  z  components  is:’,std(sunmeaserror(3,l:count)) 

’The  standard  dev  of  the  generated  mag  field  vector  x  components  is:',std(magmeaserror(l,l:count)) 
The  standard  dev  of  the  generated  mag  field  vector  y  components  is:',std(magmeaserror(2,l  :count)> 
The  standard  dev  of  the  generated  mag  field  vector  z  components  is:’,std(magmeaserror(3,l:count)) 
The  standard  dev  of  the  generated  omega  1  measurement  is:\std(omega  1  measerror(  1  :count)) 

The  standard  dev  of  the  generated  omega2  measurement  is:',std(omega2measerror(l  :count)) 

’The  standard  dev  of  the  generated  omega3  measurement  is:\std(omega3measerror(  1  :count» 

end 


'Data  simulation  complete... press  "return”  to  begin  processing  data' 
pause;close;clc; 

A.3  Omegagen_exp.m 

Omegagen_exp.m  is  a  user  defined  function  that  is  called  during  the  execution  of  attitude_sim.m,  if 
designated  by  the  operator.  This  routine  generates  body  frame  rotational  rate  profiles  that  asymptotically 
approach  a  final  value,  input  by  the  user  for  each  axis. 


%Omegagen„exp  -  user  defined  function  that  generates  omega  arrays 
%for  use  by  attitude_sim.  User  defined  omega  profiles  that 
^asymptotically  approach  the  final  desired  RPM  at  user-defined  times. 
%User  must  input  initial  omega,  ramp  variables  and  desired  final  rpm. 
% 

^function  [omegal,  omega2,  omega3,TS]=omegagen_exp(totaLtime) 
% 

%omegal  =  angular  rate  about  body  frame  x-axis 
%omega2  =  angular  rate  about  body  frame  y-axis 
%omega3  =  angular  rate  about  body  frame  z-axis 
%totaLtime  =  length  of  simulation 
% 

%  Written  by  Mark  Charlton.  Last  updated  27  October  03, 


function  [omegal,  omega2,  omega3,  TS]=omegagen_exp(totaI_time) 
clc 


^initialize  arrays 
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omega  1=[] 
omega2=[  3 
omega3=[  3 


%***#*  *******************  *************************************************** 
%******  generate  arrays  of  angular  rates  about  the  rotated  frame  axes  ****** 

^** ************************************************************************* 


%  Input  rate  profile  information  about  body  frame  x-axis,  omega  1 

omega  i=input(’Please  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  x  axis  in  rpm  (default  =  0):  '); 
if  isempty(omegal) 
omega  1=0 
end 

omega  1  final=input('Please  enter  the  final  value  of  the  angular  rate  about  the  body  frame  x  axis  in  rpm  (default  =  .5):  '); 
if  isempty(omegal  final) 
omega  lfmal=.5 
end 

omega  lramp=input(*Please  enter  the  fraction  of  the  entire  simulation  at  which  you  which  final  value  of  the  angular  rate  to  be 
achieved  (default  =  1/2):  ’); 
if  isempty(omega  1  ramp) 
omega  lramp=.5 
end 

omega2=input('Please  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  y  axis  in  rpm  (default  =  0):  '); 
if  isempty(omega2) 
omega2=0 
end 

omega2final=inputOPiease  enter  the  final  value  of  the  angular  rate  about  the  body  frame  y  axis  in  rpm  (default  =  .5):  *); 
if  isempty(omega2fmal) 
omega2final=  5 
end 

omega2ramp=input(’PIease  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  final  value  of  the  angular  rate  to  be  achieved 
(default  =  1/2):  ’); 
if  isempty(omega2ramp) 
omega2ramp=.5 
end 

omega3=input(’Please  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  z  axis  in  rpm  (default  =  0):  '); 
if  isempty(omega3) 
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omega3=0 

end 

omega3final=input('Please  enter  the  final  value  of  the  angular  rate  about  the  body  frame  z  axis  in  rpm  (default  =  225):  ’); 
if  isempty(omega3final) 
omega3final=225 
end 

omega3ramp=input('PIease  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  final  value  of  the  angular  rate  to  be  achieved 
(default  =  1/2):  '); 
if  isempty(omega3ramp) 
omega3ramp=.5 
end 

%*******  determine  highest  frequency  of  simulated  motion  ******* 
highestfreq=max([omegal  final  omega2final  omega3final])*2*pi/60; 

%*******  simulation  time  step  ************** 

'*****  Care  should  be  taken  to  choose  a  simulation/sampling  interval  at  least  3  times  highest  frequency  of  rotational  motion!!!*****' 
TS=input('Please  enter  the  attitude  parameter  simulation  interval  in  seconds  (default  =  .01  (100  s/sec)):  ’); 

if  isempty(TS) 
if  highestfreq=0 
TS=1 
else 
TS=.01 
end 
end 

%*******  generate  time  array  based  on  final  time  and  time  step  ******* 

T=0:TS:totaLtime; 
count =length(T); 

omega  l(l,l)=omegal; 
omega2(  1 , 1  )=omega2; 
omega3(  1 , 1  )=omega3 ; 


ramp  1  =floor(count*omega  1  ramp); 
ramp2=floor(cou  nt*omega2ramp); 
ramp3=floor(count*omega3  ramp) ; 
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for  i=2:count  ' 

omega  l(l,i)=(omegalfinal*2*pi/60)*(l-exp(-(5/rampl)*i));  %generate  omegal  profile 
end 

for  j=2xount 

omega2(  1  j)=(omega2final*2*pi/60)*(  1  -exp(-(5/ramp2)*j));  %generate  omega2  profile 
end 

for  k=2:count 

omega3(l,k)=(omega3final*2*pi/60)*(l-exp(-(5/ramp3)*k));  ^generate  omega3  profile 
end 

%******  pjot  anguiaf  rates  ^  the  body  frame  ****** 

%plot 

(T(  1 , 1  :count),  omega  1(1,1  xount)*60/(2*pi),’bo',T(  1 , 1  xount),omega2(  l ,  1  xount)*60/(2*pi)/rx\T(  1 , 1  xount),omega3(  1 , 1  xount)*60/(2 
*pi),'m*'); 

%grid  on;title('body  rates  versus  time'); 

%xlabel('time  (sec)');yIabeI('body  rates  (rev/min),);legendComegar,  *omega2',  *omega3’); 

%pause;close; 

%end  of  function 

A.4  Omegagen_ramp.m 

Omegagenjramp.m  is  a  user  defined  function  that  is  called  during  the  execution  of  attitude_sim.m,  if 
designated  by  the  operator.  This  routine  generates  body  frame  rotational  rate  profiles  that  linearly 
approach  a  final  value,  input  by  the  user  for  each  axis. 

%omegagen_ramp  -  user  defined  function  that  generates  omega  arrays 
%for  use  by  attitude_sim.  User  defined  omega  profiles  that  have 
%a  linear  ramp  rise  reaching  the  final  desired  RPM  at  defined  times. 

%User  must  input  initial  omega,  ramp  variables  and  desired  final  ipm. 

% 

^function  [omegal,  omega2,  omega3,TS]=omegagen_ramp(total_time) 

% 

%omegal  =  angular  rate  about  body  frame  x-axis 
%omega2  =  angular  rate  about  body  frame  y-axis 
%omega3  =  angular  rate  about  body  frame  z-axis 
%total_time  =  length  of  simulation 
% 

%  Written  by  Mark  Charlton.  Last  updated  2 1  October  03. 
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function  [omegal ,  omega2,  omega3,TS]=omegagen_ramp(total_time) 
clc 

^initialize  arrays 

omega  l-[  ]; 
omega2=[  ]; 
omega  3 =[  ]; 

^************** ************************* **;M**************** ********** *****^ 

%******  generate  arrays  of  angular  rates  about  the  rotated  frame  axes  ****** 
%*************************************************************************** 


%  Input  rate  profile  information  about  body  frame  x-axis,  omegal 

omegal  =input('Please  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  x  axis  in  rpm  (default  =  0):  ’); 
if  isempty(omega  1 ) 
omega  1=0 
end 

omega  lfinal=input(’Please  enter  the  final  value  of  the  angular  rate  about  the  body  frame  x  axis  in  rpm  (default  =  .5):  '); 
if  isempty(omegal  final) 
omega  lfinal=.5 
end 

omega  lramp=input(’Please  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  final  value  of  the  angular  rate  to  be  achieved 
(default  =  1/2:  ’); 
if  isempty(omegalramp) 
omega  lramp=.5 
end 

omega2=input('Please  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  y  axis  in  rpm  (default  =  0):  ’); 
if  isempty(omega2) 
omega2=0 
end 

omega2final=input(’PIease  enter  the  final  value  of  the  angular  rate  about  the  body  frame  y  axis  in  rpm  (default  =  .5):  ’); 
if  isempty(omega2final) 
omega2final=  5 
end 

omega2ramp=input('Please  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  final  value  of  the  angular  rate  to  be  achieved 
(default  =1/2):  ’); 
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if  isempty(omega2ramp) 
omega2ramp=  5 
end 

omega3=input('Please  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  z  axis  in  rpm  (default  =  0):  '); 
if  isempty(omega3) 
omega3=0 
end 

omega3fina!=input('Please  enter  the  final  value  of  the  angular  rate  about  the  body  frame  z  axis  in  ipm  (default  =  225):  '); 
if  isempty(omega3final) 
omega3final=225 
end 

omega3ramp=input('Please  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  final  value  of  the  angular  rate  to  be  achieved 
(default  =  1/2):  '); 
if  isempty(omega3ramp) 
omega3ramp=.5 
end 

%*******  determine  highest  frequency  of  simulated  motion  ******* 
highestfreq=max([omegal  final  omega2final  omega3final])*2*pi/60; 

%*******  simulation  time  step  ************** 

******  Care  should  be  taken  to  choose  a  simulation/sampling  interval  at  least  3  times  highest  frequency  of  rotational  motion!!!*****' 
TS=input('Please  enter  the  attitude  parameter  simulation  interval  in  seconds  (default  =  .01  (100  s/sec)):  '); 

if  isempty(TS) 
if  highestfreq=0 
TS=1 
else 
TS=.01 
end 
end 

%*******  generate  time  array  based  on  final  time  and  time  step  ******* 

T=0:TS  :total_time; 
count=length(T); 

omega  1  ( 1 , 1  )=omega  1 ; 
omega2(  1 , 1  )=omega2 ; 
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omega3(  1 , 1  )=omega3 ; 

ramp  1  =floor(count*omegal  ramp); 
ramp2=floor(count*omega2ramp); 
ramp3=floor(count*omega3ramp); 

for  i=2:rampl+l 

omega  1  ( 1  ,i)=omega  1  ( 1  ,i- 1  )+(omega  1  final*2*pi/60)/ramp  1 ;  ^generate  omega  1  profile 
end 

for  i=ramp  l  +2  :count 
omega  1  ( 1  ,i)=omega  1  ( l  ,i- 1 ); 
end 

forj=2:ramp2+l 

omega2(  1  j)=omega2(  1  ,j- 1  )+(omega2final*2*pi/60)/ramp2 ;  ^generate  omega2  profile 
end 

for  j=ramp2+2:count 
omega2(  1  ,j)=omega2(  1  j- 1 ); 
end 

for  k=2:ramp3+l 

omega3(l,k)=omega3(l,k-l)+(omega3final*2*pi/60)/ramp3;  %generate  omega3  profile 
end 

for  k=ramp3+2:count 
omega3(  1  ,k)=omega3(l,k- 1 ); 
end 

%******  plot  angular  rates  in  the  body  frame  ****** 

%p!ot 

(T(l,l:count),omegal(l,l:count)*60/(2*piX'bo',T(l,l:count),omega2(l,l:count)*60/(2*pi),,rx\T(l,l:count),omega3(l,l:count)*60/(2 

*pi),’m*'); 

%grid  on;title('body  rates  versus  time'); 

%xlabel('time  (sec)’);ylabel(’body  rates  (rev/min)’);legendComegar,  ,omega2',  'omega3'); 

%pause;close; 

%end  of  function 

A.5  Omegagen_fix.m 

Omegagen_fix.m  is  a  user  defined  function  that  is  called  during  the  execution  of  attitude__sim.m,  if 
designated  by  the  operator.  This  routine  generates  body  frame  rotational  rate  profiles  that  have  a  fixed 
value,  input  by  the  user  for  each  axis. 


%omegagen_fix  -  user  defined  function  that  generates  omega  arrays 
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%for  use  by  attitude_sim.  User  defined  omega  profiles  that  have 
%a  fixed  angular  rates.  User  must  input  desired  fixed  rpm. 

% 

%  function  [omega  1,  omega2,  omega3,TS]=omegagen_ramp(total_time) 
% 

%omegal  =  angular  rate  about  body  frame  x-axis 
%omega2  =  angular  rate  about  body  frame  y-axis 
%omega3  =  angular  rate  about  body  frame  z-axis 
%total_time  =  length  of  simulation 
% 

%Written  by  Mark  Charlton.  Last  updated  21  October  03. 


function  [omega  1,  omega2,  omega3,TS]=omegagen_fix(totaLtime) 

clc 

^initialize  arrays 

omega  1=[  ]; 
omega2=[  ]; 
omega3=[  ]; 

%*************************************************************************** 

%******  generate  arrays  of  angular  rates  about  the  rotated  frame  axes  ****** 

^******** ************************************************************* ****** 

%  Input  rate  profile  information  about  body  frame  x-axis,  omega  1 

omega  l=input('Please  enter  the  desired  value  of  the  constant  angular  rate  about  the  body  frame  x  axis  in  rpm  (default  =  .5):  ’); 
if  isempty(omegal) 
omega  1  =.5 
end 

omega  l  =omega  1  *2*pi/60; 

omega2=input('Please  enter  the  desired  value  of  the  constant  angular  rate  about  the  body  frame  y  axis  in  ipm  (default  =  .5):  '); 
if  isempty(omega2) 
omega2=.  5 
end 

omega2=omega2*2*pi/60; 


omega3=input('PIease  enter  the  desired  value  of  the  constant  angular  rate  about  the  body  frame  z  axis  in  rpm  (default  =  225):  '); 
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if  isempty(omega3) 
omega3=225 
end 

omega3=omega3*2*pi/60; 

%*******  determine  highest  frequency  of  simulated  motion  ******* 
highestfreq=max([omegal  omega2  omega3]); 

%*******  simulation  time  step  ************** 

******  Care  should  be  taken  to  choose  a  simulation/sampling  interval  at  least  3  times  highest  frequency  of  rotational  motion!!!*****' 
TS=input('PIease  enter  the  attitude  parameter  simulation  interval  in  seconds  (default  =  .01  (100  s/sec)):  '); 

if  isempty(TS) 
ifhighestfreq=0 
TS=1 
else 
TS=.01 
end 
end 

%*******  generate  time  array  based  on  final  time  and  time  step  ******* 

T=0:TS:totaLtime; 

count=Iength(T); 

omega  1(1,1  )=omega  1 ; 
omega2(  l ,  1  )=omega2 ; 
omega3(  l ,  1  )=omega3 ; 

for  i=2:count 

omegal(l,i)=omegal(l,i-l);  %generate  omega  1  profile 

end 

forj=2:count 

omega2(  l  ,j)=omega2(  1  j- 1 );  ^generate  omega2  profile 

end 

end 

for  k=2:count 

omega3(l,k)=omega3(l,k-l);  ^generate  omega3  profile 


end 


195 


%******  plot  angular  rates  in  the  body  frame  ****** 

%plot 

(T(l ,  1  :count),  omegal  (1, 1  :count)*60/(2*pi),'bo',T(l ,  1  :count),omega2(l ,  1  :count)*60/(2*pi),'rx\T(  1, 1  :count),  omega3(l ,  1  :count)*60/(2 
*pi),'m*’); 

%grid  on;title('body  rates  versus  time'); 

%xlabel('time  (sec),);ylabel(’body  rates  (rev/min)');legend(,omegar,  'omega2',  'omega3’); 

%pause;close; 

%end  of  function 

A.6  Omegagen_step.m 

Omegagen_step.m  is  a  user  defined  function  that  is  called  during  the  execution  of  attitude_sim.m,  if 
designated  by  the  operator.  This  routine  generates  body  frame  rotational  rate  profiles  that  have  a  step 
shape  with  start  and  stop  times  and  maximum  values  input  by  the  user  for  each  axis. 

%omegagen_step  -  user  defined  function  that  generates  omega  arrays 
%for  use  by  attitude„sim.  User  defined  omega  profiles  that  have 
%a  step  rise  to  the  final  desired  RPM  at  a  defined  time  and  a  step  drop 
%back  to  zero  at  another  defined  time. 

%User  must  input  initial  omega,  ramp  variables  and  desired  non-zero  rpm. 

% 

%function  [omegal,  omega2,  omega3,TS]=omegagen__step(total_time) 

% 

%omegal  =  angular  rate  about  body  frame  x-axis 
%omega2  =  angular  rate  about  body  frame  y-axis 
%omega3  =  angular  rate  about  body  frame  z-axis 
%totaLtime  =  length  of  simulation 
% 

%  Written  by  Mark  Charlton.  Last  updated  21  October  03. 


function  [omegal,  omega2,  omega3,TS]=omegagen_step(total_time) 
clc 

^initialize  arrays 

omega  1=[  ]; 
omega2=[  ]; 


omega3=[  ]; 


%*************************************************************************** 

%******  generate  arrays  of  angular  rates  about  the  rotated  frame  axes  ****** 

4^*************************************************************************** 

%Input  rate  profile  information  about  body  frame  x-axis,  omega  1 

omega l=input('PIease  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  x  axis  in  rpm  (default  =  0):  '); 
if  isempty(omegal) 
omega  1=0 
end 

omega  1  =omega  1  *2*pi/60;  %convert  to  rad/s 

omegalfinal=inputCPIease  enter  the  final  value  of  the  angular  rate  about  the  body  frame  x  axis  in  rpm  (default  =  .2):  ’); 
if  isempty(omega  1  final) 
omega  lfinal=.2 
end 

omega lfinal=omegalfinal*2*pi/60;  ^convert  to  rad/s 

omega lstepup=input(Tlease  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  pulse  to  begin  (default  =  .3):  ’); 
if  isempty(omegalstepup) 
omega  1  stepup=3/l  0 
end 

omega lstepdown=input('Please  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  pulse  to  end  (default  =  .7):  '); 
if  isempty(omegalstepdown) 
omega  1  stepdown=7/l  0 
end 

%Input  rate  profile  information  about  body  frame  y-axis,  omega2 

omega2=input(TIease  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  y  axis  in  ipm  (default  =  0):  '); 
if  isempty(omega2) 
omega2=0 
end 

omega2=omega2*2*pi/60;  ^convert  to  rad/s 

omega2final=input('PIease  enter  the  final  value  of  the  angular  rate  about  the  body  frame  y  axis  in  rpm  (default  =  .2):  '); 
if  isempty(omega2final) 
omega2final=.2 
end 


omega2final=omega2final*2*pi/60;  %convert  to  rad/s 


omega2stepup=:input(,Please  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  pulse  to  begin  (default  =  .3):  '); 
if  isempty(omega2stepup) 
omega2stepup=3/ 1 0 
end 

omega2stepdown=input('Please  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  pulse  to  end  (default  =  .7):  ’); 
if  isempty(omega2stepdown) 
omega2stepdown=7/ 1 0 
end 

%Input  rate  profile  information  about  body  frame  z-axis,  omega3 

omega3=input('Please  enter  the  initial  value  of  the  angular  rate  about  the  body  frame  z  axis  in  rpm  (default  =  0):  '); 
if  isempty(omega3) 
omega3=0 
end 

omega3=omega3*2*pi/60;  ^convert  to  rad/s 

omega3final=input('Please  enter  the  final  value  of  the  angular  rate  about  the  body  frame  z  axis  in  rpm  (default  =  .2):  '); 
if  isempty(omega3  final) 
omega3fmal=  2 
end 

omega3final=omega3final*2*pi/60;  %convert  to  rad/s 

omega3stepup=inputfPlease  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  pulse  to  begin  (default  =  .3):  '); 
if  isempty(omega3stepup) 
omega3stepup=3/10 
end 

omega3stepdown=input(,Please  enter  the  fraction  of  the  entire  simulation  at  which  you  wish  pulse  to  end  (default  =  .7):  '); 
if  isempty(omega3stepdown) 
omega3  stepdo  wn=7/ 1 0 
end 

%*******  determine  highest  frequency  of  simulated  motion  ******* 
highestfreq=max( [omega  1  final  omega2final  omega3fmal])*2*pi/60; 


%*******  simulation  time  step  ************** 
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******  Care  should  be  taken  to  choose  a  simulation/sampling  interval  at  least  3  times  highest  frequency  of  rotational  motion!!!****** 
TS=input('Please  enter  the  attitude  parameter  simulation  interval  in  seconds  (default  =  .01  (100  s/sec)):  '); 
if  isempty(TS) 

TS=0.01 

end 

%*******  generate  time  array  based  on  final  time  and  time  step  ******* 

T=0:TS:total_time; 

count=length(T); 

omega  1(1,1  )=omega  1 ; 
omega2(  1 , 1  )=omega2 ; 
omega3(  1 , 1  )=omega3 ; 

stepup  1  =floor(count*omega  1  stepup) ; 

stepdown  l=floor(count*omegal  stepdown); 

stepup2=floor(count*omega2stepup); 

stepdown2=floor(count*omega2stepdown); 

stepup3=fIoor(count*omega3stepup); 

stepdown3=floor(count*omega3stepdown); 

for  i=2:stepupl 

omega  1  ( 1  ,i)=omega  1(1,1);  ^generate  omega  1  profile 

end 

for  i=stepupl +1  istepdown  1 
omega  1  (1  ,i)=omegal  final; 
end 

for  i=stepdownl+l  rcount; 

omega  1  ( 1  ,i)=omega  1  ( 1 , 1 ); 
end 

for  j=2:stepup2 

omega2(l  j)=omega2(l,l);  %generate  omega2  profile 
end 

for  j=stepup2+ 1  :stepdo  wn2 
omega2(  l  j  )=omega2final; 
end 

for  j=stepdo  wn2+ 1  xount; 

omega2(  1  j)=omega2(  1,1); 
end 


for  k=2:stepup3 
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omega3(l  ,k)=omega3(l,  1);  ^generate  omega3  profile 
end 

for  k=stepup3+l:stepdown3 
omega3(  1  ,k)=omega3final; 
end 

for  k=stepdown3+l  :count; 

omega3  ( 1  ,k)=omega3  (1,1); 
end 


%******  plot  angular  rates  in  the  body  frame  ****** 

%p\ot 

(T(l,  1  :count),omega  1(1,1  :count)*60/(2*pi)/bo\T(  1 , 1  :count),omega2(  1 , 1  :count)*60/(2*pi),'rx\T(  1 ,1  :count),omega3(  l , 1  :count)*60/(2 
*pi)/m*'); 

%grid  on;title('body  rates  versus  time'); 

%xlabel('time  (sec)’);yIabel(’body  rates  (rev/min)’);legend(’omega  1’,  'omega2',  ’omega3’); 

%pause;close; 

%end  of  function 

A.7  Qrotm 

This  is  a  user  defined  function  that  calculates  the  familiar  Euler  rotation  axis  and  angle  representation  of  a 
rotation  from  a  given  rotation  quaternion.  It  is  called  from  attitude_sim.m  if  the  “diagnostics”  option  is 
selected. 

%qrot  -  user  defined  function  to  calculate  rotation  and  axis  corresponding 
%to  a  given  rotation  quaternion. 

% 

%  function  [axis, angle]  =qrot(q__rot) 

% 

%q_rot  =  rotation  quaternion  of  unit  magnitude 
%axis  =  Euler  axis  of  rotation 
%angle  =  angle  of  rotation  about  axis 
% 

%  Written  by  Mark  Charlton.  Last  updated  28  October  2003. 

function  [axis,angle]=qrot(q_rot) 
clc; 

’The  entered  rotation  quaternion  is: ' 
q_rot 
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q„rot=q_rot/norm(q_rot);  ^insures  unit  quaternion 
%Calculate  rotation  generated  by  q_rot 
angle=2*acos(q_rot(4)); 

angle=angle*  1 80/pi;  %convert  angle  to  degrees 

I  t 

axis=[q_rot(l)  q_rot(2)  q„rot(3)]’; 
axis=axis/sqrt(axis'*axis); 

'The  unit  vector  representing  the  Euler  axis  of  rotation  and  the  angle  of  rotation  in  degrees  are: ' 

%end  of  function 

A.8  Gauss_Newton.m 

Gauss_Newton.m  is  a  user  defined  MATLAB  function  to  implement  Gauss-Newton  error  minimization 
for  a  quadratic  error  function.  This  function  is  called  by  the  main  script  attitude_filter.m  and  operates  on 
the  noisy  measurements  coming  out  of  attitude_sim.m.  It  produces  the  “best”  quaternion  estimate  from 
the  error  minimization  process.  These  noisy  quaternion  components  are  then  operated  on  by  the  chosen 
EKF  or  UKF  filter  which  produces  the  state  estimate. 

%Gauss_Newton  -  user  defined  function  to  minimize  errors  in  transformation  between  two  given  vectors. 

%Uses  Gauss-Newton  algorithm  to  minimize  mean-square-error  (MSE)  [65  ].  The  error  function  is 
%of  the  form  (vref-M*vrot)'*(vref-M*vrot).  Function  yields  rotation  quaternion  that  describes 
%  transformation  between  vector  in  rotated  frame  and  vector  in  reference  frame  that  yields  lowest  MSE. 

% 

^function  [q_hat,  A,  check,err]=Gauss_newton(vref,  vrot,  q_init, steps, tol) 

% 

%q_hat  =  estimated  rotation  quaternion  based  on  minimized  MSE 
% 

%A  -  matrix  that  calculates  measurement  errors  of  the  four  computed 
%quatemion  elements  based  on  the  measurement  errors  of  the  six  sensor 
^measurements  [11] 

% 

%check  =  convergence  flag  for  algorithm.  check=2  indicates  normal 
^convergence.  check=0  indicates  that  algorithm  did  not  converge  in 
%1,steps"  iterations. 

% 

%vref  =  vector  in  reference  frame  (6x1) 

%vrot  =  vector  in  rotated  frame  (6  x  1) 


%q_init  =  initial  guess  at  unit  rotation  quaternion  (4x1) 

%steps  =  max  steps  to  converge 
%tol  =  convergence  tolerance 
% 

%  Written  by  Mark  Charlton.  Last  updated  21  October  03. 

function  [q Jiat,  A,  check,  err]  =  Gauss_newton(vref,  vrot,  q_init,  steps,  tol) 

^Normalize  initial  quaternion  guess  to  get  required  unit  vector 


qjnit=qjnit/sqit(qjnit,*q_init); 


%Initialize  variables 

q_hat=q_init;  %rotation  quaternion  resulting  from  minimizing  MSE 

A=zeros(4,6);  %matrix  relating  quaternion  error  to  measurement  error 

M__hat=zeros(6,6);  %transformation  matrix  in  error  function 

err=l ;  %error  used  for  convergence  criteria 

i=0;  %counter  to  prevent  infinite  loop 

check=l ;  %flag  to  indicate  status  of  convergence 

q_hatl=[qjnit(l)]; 

q_hat2=[qjnit(2)]; 

q_hat3=[q_init(3 )] ; 

qjiat4=[q_init(4)]; 

steparray=[i]; 

errarray=[err]; 

%  Iterate  to  minimize  MSE  and  yield  optimum  rotation  quaternion 

while  check— 1 

%Quatemion  rotation  matrix  describing  transformation  from  body  frame  to 
%reference  frame  [40] 

R_hat(l  ,l)=q_hat(l  )A2-q_hat(2)A2-q_hat(3)A2+q_hat(4)A2; 

R__hat(  1 ,2)=2*(q_hat(  1  )*q_hat(2)-q_hat(3)*q__hat(4)); 

R_hat(  1 ,3)=2*(q_Jiat(  1  )*q_hat(3)+q_hat(2)*q_hat(4)); 

R _hat(2, 1  )=2*(q_hat(  1  )*q_hat(2)+q_hat(3)*q_hat(4)); 
R„hat(2,2)=-q_hat(l)A2+q_hat(2)A2-q_hat(3)A2+q„hat(4)A2; 
R_hat(2,3)=2*(-q_hat(  1  )^q_hat(4)+q„hat(2)*q„hat(3)); 
RJiat(3,l)=2*(q_hat(l)*q_hat(3)-q_hat(2)*q_.hat(4)); 
R_hat(3,2)=2*(q_hat(  1  )*q_hat(4)+q_hat(2)*q_hat(3)); 

R_hat(3,3)=-q_hat(  1  )A2-q_hat(2)A2+q_hat(3)A2+q_hat(4)A2; 


%Form  matrix  M  in  error  vector  errvec=(vref-Mvrot) 


M_hat=[R_hat,  zeros(3,3);  zeros(3,3),  R_hat] ; 

%CalcuIate  partial  derivatives  to  form  Jacobian  matrix,  jacobian 

subql=[q_hat(l)  q_hat(2)  q_hat(3);  q_hat(2)  -q__hat(l)  -q_hat(4);  q_hat(3)  q_hat(4)  -q_hat(l)j; 
paitql=2*[subql,  zeros(3,3);  zeros(3,3),  subql]*vrot; 

subq2=[-qjiat(2)  q_hat(l)  q_hat(4);  q_hat(l)  q_hat(2)  q_hat(3);  -q_hat(4)  q_hat(3)  -q_hat(2)]; 
paitq2=2*[subq2,  zeros(3,3);  zeros(3,3),  subq2J*vrot; 

subq3=[-q_hat(3)  -q_hat(4)  q_hat(l);  q_hat(4)  -q_hat(3)  q__hat(2);  q_hat(l)  q_hat(2)  qjiat(3)]; 
partq3=2*[subq3,  zeros(3,3);  zeros(3,3),  subq3]*vrot; 

subq4=[q„hat(4)  -q_hat(3)  q_hat(2);  q_hat(3)  q_hat(4)  -q_hat(l);  -q_hat(2)  q_hat(l)  q_hat(4)]; 
partq4=2*[subq4,  zeros(3,3);  zeros(3,3),  subq4]*vrot; 

^Calculate  Jacobian  matrix,  jacobian 

jacobian=-[partql,  partq2,  partq3,  paitq4]; 

^Calculate  new  estimate  of  quaternion 

q_hat=q_hat-(inv(jacobian'*jacobian))*jacobian’*(vref“M_hat*vrot); 

%Normalize  estimated  quaternion  to  get  required  unit  quaternion 
q_hat=q_hat/sqrt(q_hat'*q  Jiat) ; 

%  Check  error  vector 

errvec= vref-M_hat*  vrot; 

err=(errvec'*errvec)/length(errvec);  %calculate  mean  square  error 

%increment  counter 


i=i+l; 


%Check  for  convergence  of  Gauss-Newton 


if  i  >=  steps 

check  =  0;  %set  convergence  flag 
%'Gauss-Newton  failed  to  converge  in  25  steps...' 
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%pause(l); 

%crr 

end 

if  err  <  tol 

check  =  2 ;  %set  convergence  flag 
%’Gauss-Newton  has  converged...’ 
%pause(3); 
end 


q_hat  1  =[q_hat  1 ,  qjiat(  1 )] ; 
q_hat2=[q„hat2,  qjiat(2)]; 
q_hat3=[q_hat3,  q_hat(3)]; 
q_hat4=[q_hat4,  q_hat(4)]; 
errarray=[errarray,  err]; 
stepaiTay=[stepaiTay,  i]; 


%array  containing  q_hat(l)  at  each  time  step 
%array  containing  q_hat(2)  at  each  time  step 
%array  containing  q_hat(3)  at  each  time  step 
%array  containing  q_hat(4)  at  each  time  step 
%array  containign  err  at  each  time  step 
%array  containing  all  time  steps 


end 


^Calculate  measurement  errors  of  four  computed  quaternion  elements 
%based  on  the  measurement  errors  of  the  six  sensor  measurements  [11] 

A=(inv(jacobian'*jacobian))*jacobian'*M_hat; 

%NormaIize  estimated  quaternion  to  get  required  unit  quaternion 

q_hat=q_hat/sqit(q_hat'*q_hat); 

^diagnostic  plots  of  quaternion  components 

%plot(steparray,q_hat  1  ,steparray,q_hat2,'-r\  steparray,  q_hat3,  ’-g\steparray,  q_hat4,  ’-k’),grid  on; 
%legend(’qhat(l)7qhat(2)7qhat(3)\  ,qhat(4)’);title(’attitude  quaternion  components  versus  time  step'); 
%pause;  close; 

%plot(steparrayferrarray,'-r,)>grid  on; 

%legend('MSE');titleCMagnitude  of  squared  error  at  each  time  step'); 

%pause;  close; 


%end  of  function 
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A.9  Mean„Square_Error.m 

Mean_square_error.m  is  a  user  defined  MATLAB  function  to  determine  the  mean  square  error  (MSE) 
between  two  vectors.  It  is  used  extensively  throughout  these  algorithms  to  determine  MSE  as  a  measure 
of  method  performance. 


%Mean_square_error  -  user  defined  function  to  calculate  mean  square  error  between  two 
%vectors. 

% 

%  function  [mean_square_error]=Mean_square_error(vec  1 ,  vec2) 

% 

%mean_square_error  =  mse  between  two  vectors  of  interest 
%vecl,  vec2  =  two  vectors  of  interest 
% 

%  Written  by  Mark  Charlton.  Last  updated  21  October  03. 
function  [mean_square_enor]=Mean„square„error(vecl,vec2) 
sum=0; 

for  i=l:length(vecl) 
diffsquared=(vec  1  (i)- vec2(i)).  A2; 
sum=sum+diffsquared; 
end 

mean_square_error=sum/length(vec  1 ); 

A.10  Qframerot6«m 

Qffamerot6.m  is  a  user  defined  MATLAB  function  to  take  a  six-element  input  vector  through  a  frame 
rotation  and  output  the  rotated  vector. 


%Qframerot6  -  user  defined  function  to  accomplish  a  frame  rotation  through  the 
^multiplication  of  quaternions.  Yields  original  6D  vector,  expressed  in  the 
^coordinates  of  the  rotated  frame,  when  provided  with  unitary  rotation  quaternion. 
% 

%fiinction  [vrot]=qframerot6(q_rot,  vinit) 

% 

%q_rot  =  rotation  quaternion  of  unit  magnitude 
% 

%vrot  =  6D  vector  expressed  in  the  rotated  frame 
%vinit  =  6D  vector  expressed  in  original  frame 
% 


%  Written  by  Marie  Chariton.  Last  updated  28  October  03. 


function  [vrot]  =  qframerot6(q_rot,  vinit) 

^Calculate  rotation  generated  by  q_rot 

%alpha=2  *acos(q_rot(4)); 

%clc 

%'Transpose  of  vector  in  the  original  frame  is:  \  vinit' 

%'Rotation  to  be  accomplished  in  degrees:  \alpha*  180/pi 

%axis=[q_rot(l)  q„rot(2)  q_rot(3)j'; 

%axis=axis/sqrt(axis'*axis); 

%’The  transpose  of  the  unit  vector  representing  the  Euler  axis  of  rotation:  \ axis' 

%Quatemion  direction  cosine  matrix  describing  transformation  from 
%rotated  frame  to  reference  frame  [41,  pg  7  -  9] 

%R_hat(l ,  l  )=q_rot(l  )A2-q_rot(2)A2-q_rot(3)A2+q_rot(4)A2; 

%R_hat(  1 ,2)=2*(q_rot(  1  )*q_rot(2)-q_rot(3)*q_rot(4)); 

%R_hat(  1 ,3)=2*(q_rot(  1  )*q_rot(3)+q_rot(2)*q_rot(4)) ; 

%R_hat(2, 1  )=2*(q_rot(  1  )*q_rot(2)+q_rot(3)*q_rot(4)>; 
%R_hat(2,2)=-q_rot(l)A2+q_rot(2)A2-q_rot(3)A2+q_rot(4)A2; 
%R_hat(2,3)=2*(-q_rot(  1  )*q_rot(4)+q_rot(2)*q_rot(3)); 

%R_hat(3, 1  )=2*(q_rot(  1  )*q_rot(3)-q_rot(2)*q_rot(4)); 

%R_hat(3 ,2)=2  *  (q_rot(  1  )*q_rot(4)+q_rot(2)*q_rot(3)) ; 

%R_hat(3 ,3)=-q_rot(  1 )  A2-q_rot(2)A2+q_rot(3)  A2+q_rot(4)A2 ; 

^Transpose  gives  frame  rotation  versus  vector  rotation  (describes 
^transformation  from  reference  frame  to  rotated  frame. ..ehat=R*Ehat 

%R_hat=R_hat'; 

%M_hat=[R_hat  zeros(3,3);zeros(3,3)  R_hat]; 

^Rotation  using  equivalent  matrix 
%vrot_matrix=M_hat*  vinit; 


%’The  rotated  vector,  expressed  in  the  original  frame,  using  an  equivalent  matrix  is 
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%Define  conjugate  of  q_rot 
q_rot_conj=[-q_rot(l)  ~q^rot(2)  -q_rot(3)  q_rot(4)]’; 

%Augmcnt  vector  with  zeros  to  form  two  pure  quaternions  from  two  3D  vectors  in  vinit 
VJNIT=[vinit(l  :3);0;vinit(4:6);0] ; 

^Rotation  using  quaternion  multiplication 

VrNrrprimeA=qmult(q„rot„conj,  VINIT(1 :4));  %rotate  first  half  of  vector 
VROTA=qmuIt(VINrrprimeA,  q_rot); 

VINITprimeB=qmult(q_rot_conj,  VINIT(5:8));  %rotate  second  half  of  vector 
VROTB=qmuIt(VINITprimeB,  q_rot); 

%PuU  3D  vector  from  pure  quaternion 

vrot=[VROTA(l)  VROTA(2)  VROTA(3)  VROTB(l)  VROTB(2)  VROTB(3)]’; 

%’The  original  vector,  expressed  in  the  coordinates  of  the  rotated  frame,  is:  * 

%end  of  function 

A.ll  Qvecrot3.m 

This  user  defined  function  takes  an  initial  three-element  vector  and  rotates  the  vector  according  to  an  input 
rotation  quaternion.  The  new  vector  is  represented  in  the  coordinates  of  the  original  reference  frame. 

%qvecrot3  -  user  defined  function  to  accomplish  a  3D  vector  rotation  through  the 
^multiplication  of  quaternions.  Yields  rotated  3D  vector,  expressed  in  the 
^coordinates  of  the  original  frame,  when  provided  with  unitary  rotation  quaternion. 

% 

^function  [vrot]=qvecrot3(q_rot,  vinit) 

% 

%q_rot  =  rotation  quaternion  of  unit  magnitude 

%vrot  =  rotated  3D  vector  expressed  in  original  frame 
%  vinit  =  3D  vector  to  be  rotated,  expressed  in  original  frame 

^Written  by  Mark  Charlton.  Last  updated  22  October  03. 


function  [vrot]  =  qvecrot3(q„rot,  vinit) 
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%Calculate  rotation  generated  by  q„rot 

alpha=2*acos(q_rot(4)); 

clc 

%'Vector  to  be  rotated  is:  \  vinit 

%'Rotation  to  be  accomplished  in  degrees:  alpha*  180/pi 

axis=[q_rot(l)  q_rot(2)  q_rot(3)]'; 
axis=axis/sqrt(axis'*axis); 

%The  unit  vector  representing  the  Euler  axis  of  rotation:  \axis 

%Quatemion  direction  cosine  matrix  describing  transformation  from 
%rotated  frame  to  reference  frame  [[40],  pg  7  -  9] 

R_hat(  1 , 1  )=q_rot(  1  )A2-q_rot(2)A2-q_rot(3)A2+q_rot(4)A2; 

R_hat(  1 ,2)=2*(q_rot(  1  )*q_rot(2)-q_rot(3)*q_rot(4)); 

R_hat(  l,3)=2*(q_rot(  1  )*q_rot(3)+q_rot(2)*q_rot(4)); 

R_hat(2, 1  )=2*(q_rot(  1  )*q_rot(2)+q_rot(3)*q_rot(4)); 

R_hat(2,2)=-q_rot(  1  )A2+q_rot(2)A2-q_rot(3)A2+q_rot(4)A2; 

R_hat(2,3)=2*(-q_rot(  1  )*q_rot(4)+q_rot(2)*q_rot(3)); 

R__hat(3,  l)=2*(q„rot(  1  )*q_rot(3)-q_rot(2)*q_rot(4)); 

R_hat(3,2)=2*(q_rot(  1  )*q^rot(4)+q_rot(2)*q_rot(3)); 

R_hat(3,3)=-q_rot(  1  )A2-q_rot(2)A2+q_rot(3)A2+q_rot(4)A2; 

^Rotation  using  equivalent  matrix 

vrot_matrix=R_hat*vinit; 

%’The  rotated  vector,  expressed  in  the  original  frame,  using  an  equivalent  matrix  is:  vrot_matrix 
%’  ' 


%Define  conjugate  of  q_rot 
q_rot_conj~[-q_rot(  1 )  -q_rot(2)  -q_rot(3)  q_rot(4)]‘; 
%Form  pure  quaternion  from  3D  vector  vinit 
VINIT=[vinit;0J; 


%Vector  rotation  using  quaternion  multiplication 
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VINITprime=qmult(q_rot,  VINIT); 

VROT=qmult(VINITprime,  q_rot_conj); 

%Pu\i  3D  vector  from  pure  quaternion 

vrot=[VROT(l)  VROT(2)  VROT(3)j'; 

%The  rotated  vector,  expressed  in  the  original  frame: ' 

%end  of  function 

A.12  Orientation3«m 

This  user-defined  function  determines  the  inertial  orientation  of  the  body-fixed  frame  after  rotation  by 
both  a  “true”  attitude  quaternion  and  by  an  estimated  quaternion.  It  then  calculates  the  angle  between  each 
of  the  axes  rotated  by  each  method  to  form  “error  angles.”  This  function  is  called  by  norates_ekf.m, 
norates_ukf.m,  rates_ekf.m,  or  by  rates__ukf.m  if  the  option  is  selected  to  calculate  error  angles. 


%Orientation3  -  user  defined  function  that  determines  the  inertial  orientation 
%of  the  body  frame  using  both  an  estimated  rotation  quaternion  and  a  "true" 

^quaternion.  Body  frame  vectors  are  expressed  in  inertia!  cartesian 
%coordinates.  Also  calculates  "error  angles"  between  body  frame  unit 
%  vectors  rotated  using  estimated  quaternion  and  those  rotated  using  "true" 

%quatemion. 

% 

%function  [ovec  1  ,ovec2,ovec3,ovec lhat,ovec2hat,ovec3hat,angl ,ang2,ang3]=orientation3(qtrue,  qhat) 

% 

%ovecl  -  body  frame  "x"  axis  unit  vector  rotated  using  "true"  quaternion  and  expressed  in  inertial  coordinates 
%ovec2  -  body  frame  "y"  axis  unit  vector  rotated  using  "true"  quaternion  and  expressed  in  inertial  coordinates 
%ovec3  -  body  frame  "z"  axis  unit  vector  rotated  using  "true"  quaternion  and  expressed  in  inertial  coordinates 
%ovechatl  -  body  frame  "x"  axis  unit  vector  rotated  using  estimated  quaternion  and  expressed  in  inertial  coordinates 
%ovechat2  -  body  frame  "y"  axis  unit  vector  rotated  using  estimated  quaternion  and  expressed  in  inertial  coordinates 
%ovechat3  -  body  frame  "z"  axis  unit  vector  rotated  using  estimated  quaternion  and  expressed  in  inertial  coordinates 
%angl  -  angle  between  "x"  body  axis  unit  vector  rotated  using  "true"  and  estiamated  quaternion 
%ang2  -  angle  between  "y"  body  axis  unit  vector  rotated  using  "true"  and  estiamated  quaternion 
%ang3  -  angle  between  "z"  body  axis  unit  vector  rotated  using  "true"  and  estiamated  quaternion 
%qtrue  -  array  with  true  quaternions  as  columns 
%qhat  -  array  with  estimated  quaternions  as  columns 
% 

%Written  by  Mark  Charlton.  Last  updated  3 1  October  2003. 


function  [ovec l ,ovec2,ovec3,ovec  1  hat,ovec2hat,ovec3hat,angl ,ang2,ang3]=orientation3(qtrue,  qhat) 


format  compact 
omat=[  ]; 
omathat=[  ]; 

body!=[l  00]’; 
body2=[0  1  0]'; 
body3=[0  0  1]’; 

forj=l:Iength(qhat) 

ovec  1(1 :3  j)=qvecrot3(qtrue(  1 :4  j),body  1 ); 
ovec2(l  :3  j)=qvecrot3(qtrue(  1 :4  j),body2); 
ovec3(l:3j)=qvecrot3(qtrue(l:4j),body3); 
ovec  1  hat(  1 :3,j)=qvecrot3(qhat(  1 :4  j),body  1); 
ovec2hat(  1 :3,j)=qvecrot3(qhat(  1 :4  j),body2); 
ovec3hat(  1 :3,j)=q  vecrot3(qhat(  1 :4,j),body3); 
end 

%***************  p|otting  ^  output  statements  ***************** 

PLOTS=input('Enter  ”1"  to  display  plots.  Enter  "0"  for  no  plots.  Default  is  no  plots:  ’); 
if  isempty(PLOTS) 

PLOTS=0; 

end 


if  PLOTS==l 

%normalize  estimates  of  orientation  vectors  for  ease  of  plot  interpretation 
%  for  k=  1  :Iength(ovec  1  hat) 

%  ovec  lhat(l  :3,k)=oveclhat(  1 :3,k)./norm(ovec  lhat(  1 :3,k)); 

%  ovec2hat(l  :3,k)=ovec2hat(l  :3,k)./norm(ovec2hat(l  :3,k»; 

%  ovec3hat(l  :3,k)=ovec3hat(l  :3,k)./norm(ovec3hat(l  :3,k)); 

%  end 


pIot3(ovecl(l,:),ovecl(2,:),ovecl(3,:),’r');hold  on;grid  on;plot3(oveclhat(l,:),oveclhat(2,:),oveclhat(3,:),'r+') 
plot3(ovec2(l,:),ovec2(2,:),ovec2(3,:),'g');hold  on;grid  on;plot3(ovec2hat(l,:),ovec2hat(2,:),ovec2hat(3,:),'g+') 
plot3(ovec3(l,:),ovec3(2,:),ovec3(3,:),'o');hold  on;grid  on;plot3(ovec3hat(l,:),ovec3hat(2,:),ovec3hat(3,:),'b') 
title('3D  plot  of  "true"  and  "estimated"  body  axes  in  inertial  frame'); 
xlabel('inertial  "X"  axis');ylabel('inertial  "Y"  axis');zlabel('inei1ial  "Z"  axis'); 

legend('real  body  x  axis’, 'est.  body  x  axis’/real  body  y  axisVest.  body  y  axis', 'real  body  "z"  axis', 'est  body  "z"  axis 


'press  "enter"  to  continue...' 
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pause;close;clc; 

end 

%calculate  angles  between  body  axes  rotated  using  ’'true”  quaternion  and 
%using  "estimated"  quaternion  at  each  time  step. 

for  j=l  :length(ovecl) 

angl  (j)=acos(dot(ovec  1  ( 1 :3  j),o  vec  1  hat(  1 :3  j  ))/(norm(o  vec  1  ( 1 :3  j))*norm(ovecl  hat(l  :3  j))))*  1 80/pi; 
ang2(j)=acos(dot(ovec2(  1 :3,j),ovec2hat(l  :3  j))/(norm(ovec2(l  :3  j))*norm(ovec2hat(l  :3j))))*  1 80/pi; 
ang3(j)=acos(dot(ovec3(l  :3  j),ovec3hat(l  :3  j))/(norm(ovec3(l  :3  j))*norm(ovec3hat(l  :3  j))))*  1 80/pi; 
end 

'mean  angl',  mean  (angl) 

'std  angl\std(angl) 

’max  angl',max(angl) 

’mean  ang2',mean(ang2) 

'std  ang2\std(ang2) 

’max  ang2\max(ang2) 

'mean  ang3',mean(ang3) 

'std  ang3\std(ang3) 

'max  ang3\max(ang3) 


'processing  complete* 
pause  ;close;clc 


%end  of  function 


A.13  Norates_ekf.m 


Norates_ekf.m  is  the  MATLAB  script  that  implements  the  extended  Kalman  filter  (EKF)  designed  in 
Chapter  9.  This  implementation  is  for  the  case  where  rotational  rates  are  not  directly  measured,  but  are 
derived  from  quaternion  rates.  Norates_ekf.m  is  called  by  attitude_filter.m  if  the  appropriate  option  is 
selected  by  the  user. 

%NORATES_EKF  -  Discrete  Extended  Kalman  Filter  Implementation 
%Uses  data  generated  by  attitude_truth.m  to  estimate  3-D  body  rates  and 
%the  attitude  quaternion  of  a  sounding  rocket  given  noisy  measurements 
%from  two  attitude  sensors.  Uses  a  Gauss-Newton  optimization  and  an 
%Extended  Kalman  Filter  (EKF)  to  generate  a  best  estimate. 

% 


^Written  by  Mark  Charlton.  Last  modified  28  October  03. 
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%Object  subject  to  rotational  motion  —  non-linear  Equation  of  Motion 
%*****  initialize  parameters  and  matrices  ***** 

clc 


ics=[  1; 

sigmas=[  ]; 
error=[  ]; 
sensorerror=[  ]; 

ArrayT=[  ]; 
qlhat=[  ]; 
q2hat=[]; 
q3hat=[  ]; 
q4hat=[  ]; 
omega  lhat=[  ]; 
omega2hat=[  ]; 
omega3hat=[  ]; 

ArraySPll=[  ]; 

ArraySPllP=[  ]; 

ArrayS  P22=[  ]; 

ArrayS P22P=[  ]; 

ArraySP33=[  ]; 

ArraySP33P=[  3; 

ArraySP44=[  ]; 

ArraySP44P=[  ]; 

ArrayS  P5  5 =[  ]; 

ArrayS  P55P=[  ]; 

Array  SP66=[  ]; 

ArraySP66P=[  ]; 

ArraySP77=[  ]; 

ArraySP77P=[  ]; 

HI  =.01; 
statesize=7; 

PHIS=0; 

PHI=zeros(statesize,statesize); 

Q=zeros(statesize,statesize); 

P=zeros(statesize,statesize); 

HMAT=zeros(statesize,statesize); 


^initialize  error  multiplier 
^initialize  error  multiplier 
^initialize  Arrays 


%assign  integration  step-size  for  Euler  integration 
%system  order 

%process  noise  (measure  of  uncertainty  in  dynamic  model) 

^initialize  state  transition  matrix,  PHI 

^initialize  process  noise  matrix 

%initialize  covariance  matrix 

^initialize  linearized  measurement  matrix 


%*****  Assign  initial  estimates  for  omegalhat,  omega2hat,  omega3hat,  qlhat,  q2hat,  q3hat,  q4hat  ***** 


’Do  you  wish  to  enter  initial  estimates  of  the  rotational  body  rates  and  the  quaternion  components’ 
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’or  let  the  program  use  the  actual  initial  conditions  with  a  user  defined  percent  error  to' 

’make  an  initial  estimate  (default  is  to  let  the  program  do  it)7 

’If  you  choose  manual  entry,  an  uncertainty  in  the  estimate  is  also  required.  The  default  is  ’’auto’’.* 

ics=input('Enter  "1“  for  manual  entry  and  "0"  for  automatic:  '); 
if  isempty(ics) 
ics=0; 
end 

if  ics  =  0 


%*****  base  initialization  on  actual  intial  conditions  with  user  defined  standard  error  ***** 
clc 

error=input(’Enter  percentage  error  (+/-)  in  initial  conditions  (i.e.,  -5.2%  =  ”-5.2")  Default  is  5%:  '); 
if  isempty(error) 
error=5; 
end 

’percent  error  applied  to  the  initial  conditions  is:  *,error 
error=error/100; 

omega  1  hat(  1  )=omega  1  ( 1  )+omega  1  ( 1  )*error; 

omega2hat(  1  )=omega2(  1  )+omega2(  1  )*error; 
omega3  hat(  l  )=omega3  ( 1  )+omega3(  1  )*error; 
ql  hat(  1  )=q  1  ( 1  )+q  1  ( l  )*error; 
q2hat(  1  )=q2(l  )+q2(  1  )*error; 
q3hat(  1  )=q3(  1  )+q3(  1  )*error; 
q4hat(  1  )=q4(  1  )+q4(  1  )*error; 

'apriori  estimate  of  omega  1  (rad/s):  \omegalhat(l) 

'apriori  estimate  of  omega2  (rad/s):  \omega2hat(l) 

’apriori  estimate  of  omega3  (rad/s):  \omega3hat(l) 

'apriori  estimate  of  ql:  ’,qlhat(l) 

'apriori  estimate  of  q2:  \q2hat(l) 

'apriori  estimate  of  q3:  ’,q3hat(l) 

'apriori  estimate  of  q4:  \q4hat(l) 

%*****  Non-zero  entries  of  initial  covariance  matrix  for  auto  initial  cond  ***** 


%apriori  estimate  of  omega  1 
%apriori  estimate  of  omega2 
%apriori  estimate  of  omega3 
%apriori  estimate  of  ql 
%apriori  estimate  of  q2 
%apriori  estimate  of  q3 
%apriori  estimate  of  q4 


P(  1 , 1  )=(omega  1  ( 1  )*error+eps)  A2 ; 
P(2,2)=(omega2(  l  )*error+eps)A2; 


%set  at  square  of  uncertainty  in  omega  1 
%set  at  square  of  uncertainty  in  omega2 
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P(3,3)=(omega3(  l)*error+eps)A2; 
P(4,4)=(q  1  ( 1  )*error+eps)  A2; 
P(5,5)=(q2(  1  )*error+eps)A2; 
P(6,6)=(q3(  1  )*error+eps)A2; 
P(7,7)=(q4(l)*error+eps)A2; 


%set  at  square  of  uncertainty  in  omega3 
%set  at  square  of  uncertainty  in  ql 
%set  at  square  of  uncertainty  in  q2 
%set  at  square  of  uncertainty  in  q3 
%set  at  square  of  uncertainty  in  q4 


else 


%...or  can  manually  enter  whatever  intialization  values  you  wish... 

omegalhat(l)=input(’Please  enter  initial  omega  I  in  rpm:  '); 
omega2hat(l)=:input('Please  enter  initial  omega2  in  rpm:  '); 
omega3hat(l)=dnput(’Please  enter  initial  omega3  in  rpm:  '); 
qlhat(lHnput('Please  enter  initial  ql:  ’); 
q2hat(l)=input(,PIease  enter  initial  q2:  '); 
q3hat(l)=input('Please  enter  initial  q3:  '); 
q4hat(l)=input('Please  enter  initial  q4:  '); 


%apriori  estimate  of  omega  1 
%apriori  estimate  of  omega2 
%apriori  estimate  of  omega3 
%apriori  estimate  of  ql 
%apriori  estimate  of  q2 
%apriori  estimate  of  q3 
%apriori  estimate  of  q4 


omega  1  hat(  1  )=omega  1  hat(  1  )*2  *pi/60; 
omega2hat(  1  )=omega2hat(  1  )*2*pi/60; 

omega3hat(  1  )=omega3hat(  1  )*2*pi/60;  %con vert  manual  omega  entries  to  rad/s 

'For  manual  entry  of  initial  conditions,  estimated  uncertainty  in  the  initial  conditions' 

’is  necessary  to  intialize  the  covariance  matrix...' 


domegalhat=input('Please  enter  estimated  uncertainty  in  initial  omega  1  (rad/s):  ') 
domega2hat=inputCPIease  enter  estimated  uncertainty  in  initial  omega2  (rad/s):  ') 
domega3hat=input('PIease  enter  estimated  uncertainty  in  initial  omega3  (rad/s):  ’) 
dqlhat=input('Please  enter  estimated  uncertainty  in  initial  ql :  '); 
dq2hat=input(*Please  enter  estimated  uncertainty  in  initial  q2:  *); 
dq3hat=input(*Please  enter  estimated  uncertainty  in  initial  q3:  '); 
dq4hat=input('Please  enter  estimated  uncertainty  in  initial  q4:  '); 


%uncertainty  in  initial  estimate  of  omega  1 
%uncertainty  in  initial  estimate  of  omega2 
%  uncertainty  in  initial  estimate  of  omega3 
^uncertainty  in  initial  estimate  of  ql 
^uncertainty  in  initial  estimate  of  q2 
%uncertainty  in  initial  estimate  of  q3 
%uncertainty  in  initial  estimate  of  q4 


%*****  Non-zero  entries  of  initial  covariance  matrix  for  manual  initial  cond  ***** 


P(  1 , 1  )-(domega  1  hat+eps)A2; 
P(2,2)=(domega2hat+eps)A2; 
P(3,3)=(domega3hat+eps)A2; 
P(4,4)=(dq  1  hat+eps)  A2 ; 

P(5, 5  )=(dq2hat+eps)  A2 ; 
P(6,6)=(dq3hat+eps)A2; 

P(7 ,7  )=(dq4hat+eps)  A2 ; 


%set  at  square  of  uncertainty  in  initial  estimate  of  omega  1 
%set  at  square  of  uncertainty  in  initial  estimate  of  omega2 
%set  at  square  of  uncertainty  in  initial  estimate  of  omega3 
%set  at  square  of  uncertainty  in  initial  estimate  of  q3 
%set  at  square  of  uncertainty  in  initial  estimate  of  q3 
%set  at  square  of  uncertainty  in  initial  estimate  of  q3 
%set  at  square  of  uncertainty  in  initial  estimate  of  q4 


end 
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pause;clc 

%insure  initial  quaternion  estimate  is  a  unit  quaternion 

mag=sqrt(q  1  hat(l  )A2+q2hat(  1  )A2+q3hat(  l  )A2+q4hat(  1  )A2); 

q  1  hat(  1  )=q  1  hat(  1  )/mag; 

q2hat(  1  )=q2hat(  1  )/mag; 

q3hat(  1  )=q3hat(  1  )/mag; 

q4hat(  1  >=q4hat(  1  )/mag; 

%*****  Enter  measurement  noise  values  for  use  in  calculating  Measurement  Noise  Matrix,  R  ***** 

'Do  you  wish  to  enter  sensor  measurement  sigmas  manually  or  let  the  program  use  the  actual' 

'sensor  measurement  sigmas  with  a  user  defined  percent  error  to  make  an  initial  estimate* 

'(default  is  to  let  the  program  do  it)?' 

sigmas=input('Enter  "1"  for  manual  entry  and  "0"  for  automatic:  '); 
if  isempty(sigmas) 
sigmas=0; 
end 
clc 

if  sigmas  =  0 

%*****  base  sensor  noise  matrix  on  actual  sigmas  plus  user  defined  error  ***** 

sensorerror=input('Enter  percentage  error  (+/-)  in  sensor  measurement  sigmas  (i.e.,  -5.2%  =  "-5.2")  Default  is  0%:  ’); 
if  isempty(sensorerror) 
sensorerroi^O; 
end 

'percent  error  applied  to  the  actual  sensor  measurement  sigmas  is:  ',sensorerror 
sensorerror=sensorerror/l  00; 

SIGOMEGAl=stdomegal ;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

SIGOMEGA2=stdomega2;  %right  now.. .calculated  in  attitude_filter  from  std  of  actual  error 

SIGOMEGA3=stdomega3;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

estSIGOMEGAl  =SIGOMEGAl  +SIGOMEGA 1  *sensorerror; 

estSIGOMEGA2=SIGOMEGA2+SIGOMEGA2*sensorerror; 

estSIGOMEGA3=SIGOMEGA3+SIGOMEGA3*sensorerror; 

estSIGSUNX=SIGSUNX+SIGSUNX*sensorerror; 

estSIGSUNY=SIGSUNY+SIGSUNY*sensorerror; 

estSIGSUNZ=SIGSUNZ+SIGSUNZ*sensorerror; 

estS  IGM  AGX=S  IGM  AGX+S IGM  AGX*  sensorerror; 
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estS  IGM  AGY=S  IGM  AGY+S IGM  AGY*sensorerror; 
estSIGMAGZ=SIGMAGZ+SIGMAGZ*sensorerror, 


*apriori  estimate  of  omegal  "measurement”  sigma  (rad/s):  \estS  IGOMEG  A 1 
'apriori  estimate  of  omega2  "measurement”  sigma  (rad/s):  \estSIGOMEGA2 
'apriori  estimate  of  omega3  "measurement"  sigma  (rad/s):  \estS  IGOMEG  A3 
’apriori  estimate  of  sun  sensor  x  axis  measurement  sigma  (deg):  \estSIGSUNX*  180/pi 
'apriori  estimate  of  sun  sensor  y  axis  measurement  sigma  (deg):  \estSIGSUNY*  180/pi 
'apriori  estimate  of  sun  sensor  z  axis  measurement  sigma  (deg):  ’,estSIGSUNZ*  180/pi 
'apriori  estimate  of  mag  field  sensor  x  axis  measurement  sigma  (deg):  '.estSIGM AGX*  1 80/pi  %apriori  estimate  of  SIGMAGX 
'apriori  estimate  of  mag  field  sensor  y  axis  measurement  sigma  (deg):  \estSIGM  AGY*  1 80/pi  %apriori  estimate  of  SIGMAGY 
'apriori  estimate  of  mag  field  sensor  z  axis  measurement  sigma  (deg):  \estSIGMAGZ*  180/pi  %apriori  estimate  of  SIGMAGZ 


%apriori  estimate  of  SIGOMEGA1 
%  apriori  estimate  of  SIGOMEGA2 
%apriori  estimate  of  SIGOMEGA3 
%apriori  estimate  of  SIGSUNX 
%apriori  estimate  of  SIGSUNY 
%apriori  estimate  of  SIGSUNZ 


else 


%...or  can  manually  enter  whatever  intialization  values  you  wish... 

estSIGOMEGAl=input('Please  enter  sigma  for  the  omegal  derived  "measurement"  in  rpm:  ’);  %apriori  estimate  of  SIGOMEGA1 
estSIGOMEGA2=input('Please  enter  sigma  for  the  omega2  derived  "measurement"  in  rpm:  ');  %apriori  estimate  of  SIGOMEGA2 
estS  IGOMEG  A3=input('Please  enter  sigma  for  the  omega3  derived  "measurement"  in  rpm:  ');  %apriori  estimate  of  SIGOMEGA3 

estSIGOMEGA  1  =estS IGOMEG A 1  *2 *pi/60;  ^convert  estSIGOMEGAi  to  rad/s 

estSIGOMEGA2=estSIGOMEGA2*2*pi/60;  %convert  estSIGOMEGA2  to  rad/s 

estSIGOMEGA3=estSIGOMEGA3*2*pi/60;  ^convert  estS  IGOMEG  A3  to  rad/s 

estSIGSUNX=input(’Plea$e  enter  sigma  for  the  sun  sensor  x  axis  measurement  in  deg:  ');  %apriori  estimate  of  SIGSUNX 
estSIGSUNY=input('Please  enter  sigma  for  the  sun  sensor  y  axis  measurement  in  deg:  ');  %apriori  estimate  of  SIGSUNY 
estSIGSUNZ=input('PIease  enter  sigma  for  the  sun  sensor  z  axis  measurement  in  deg:  ');  %apriori  estimate  of  SIGSUNZ 
estS  IGM  AGX=input('PIease  enter  sigma  for  the  mag  field  sensor  x  axis  measurement  in  deg:  ');%apriori  estimate  of  SIGMAGX 
estSIGMAGY=input(’Please  enter  sigma  for  the  mag  field  sensor  y  axis  measurement  in  deg:  ');%apriori  estimate  of  SIGMAGY 
estS  IGM  AGZ=input('PIease  enter  sigma  for  the  mag  field  sensor  z  axis  measurement  in  deg:  ');%apriori  estimate  of  SIGMAGZ 


estSIGSUNX=estSIGSUNX*pi/180; 
estSIGSUNY=estSIGSUNY*pi/l  80; 
estSIGSUNZ=estSIGSUNZ*pi/180; 
estSIGMAGX=estSIGMAGX*pi/l  80; 
estSIGMAGY=estSIGMAGY*pi/180; 
estSIGM  AGZ=estSIGM  AGZ*pi/l  80; 


%convert  from  degrees  to  radians 
^convert  from  degrees  to  radians 
^convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 
^convert  from  degrees  to  radians 
^convert  from  degrees  to  radians 


end 


'Please  press  "return"  to  continue...’ 
pause;c!c;close; 
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%*****  Non-zero  submatrices  of  Measurement  Noise  Matrix,  R  ***** 

ROMEGA=diag([estSIGOMEGAl  A2+eps  estSIGOMEGA2A2+eps  estSIGOMEGA3A2+eps]); 

RSENSOR=diag([estSIGSUNXA2+eps  estSIGSUNYA2+eps  estSIGSUNZA2+eps  estSIGMAGXA2+eps  estSIGMAGYA2+eps 
estSIGMAGZA2+epsj); 

%*****  user  input  of  process  noise  to  reflect  uncertainty  in  dynamic  model,  PHIS  ***** 

confidence=inputCEnter  process  noise  to  reflect  uncertainty  in  model  (default  is  3.53):  '); 

if  isempty(confidence) 
confidence^. 53; 
end 


PHIS=confidence;  %( 1 00-confidence)/ 1 00; 


%[\1]  pg.  317 


’Process  noise  applied  is:  \PHIS 

’press  "return''  to  continue...’ 
pause;c!c;close; 

%*****  loop  to  calculate  estimated  states  at  each  time  step  ***** 
count=l; 

t=0.; 

ArrayT(l)=t; 


%initialize  counter 

%  initialize  time 


%*****  time  constants  and  noise  variances  for  noise  driving  motion  ***** 
%*****  right  now,  the  time  constants  are  hard-coded  ***** 

'enter  time  constants  associated  with  motion...’ 


tauomegal  =  input('enter  tau  omega  1  (defau!t=29)  ’); 
if  isempty(tauomegal) 
tauomegal=29 
end 

tauomega2  =  input('enter  tau  omega2  (default  =  29)  '); 
if  isempty(tauomega2) 
tauomega2=29 
end 

tauomega3  =  inputCenter  tau  omega3  (default=le7)  ’); 
if  isempty(tauomega3) 


tauomega3=le7 

end 

while  count<=length(omega  l_q_min)- 1 


%*****  Assign  variables  to  more  managable  variable  names  ***** 


xl  =omega  1  hat(count); 

x2=omega2hat(count); 

x3=omega3hat(count); 

x4=qlhat(count); 

x5=q2hat(count); 

x6=q3hat(count); 

x7=q4hat(count); 

^convenient  compilation  of  terms  to  be  used  later 
^convenient  compilation  of  terms  to  be  used  later 
^convenient  compilation  of  terras  to  be  used  later 
^convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 
^convenient  compilation  of  terms  to  be  used  later 


den=sqrt(x4A2+x5A2+x6A2+x7A2); 

den3=denA3; 

f4=(xl  *x7-x2*x6+x3*x5); 
f5=(x  l  *x6+x2*x7-x3  *x4); 
f6=(-x  1  *x5+x2*x4+x3*x7); 
f7=(-xl  *x4-x2*x5-x3*x6); 


%*****  Calculate  elements  of  linearized  State  Transition  Matrix,  F  ***** 


F(l,l)=-l/tauomegal;  %dxlxl 

F(l,2)=0;  %dxlx2 

F(l,3)=0;  %dxlx3 

F(l,4)=0;  %dxlx4 

F(l,5)=0;  %dxlx5 

F(l,6)=0;  %dxlx6 

F(l,7)=0;  %dxlx7 

F(2, 1  )=0;  %dx2xl 

F(2,2)=-l/tauomega2;  %dx2x2 

F(2,3)=0;  %dx2x3 

F(2,4)=0;  %dx2x4 

F(2,5)=0;  %dx2x5 

F(2,6)=0;  %dx2x6 

F(2,7)=0;  %dx2x7 

F(3,l)=0;  %dx3xl 

F(3,2)=0;  %dx3x2 

F(3,3)=-l/tauomega3;  %dx3x3 

F(3,4)=0;  %dx3x4 

F(3,5)=0;  %dx3x5 
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F(3,6)=0;  %dx3x6 

F(3,7)=0;  %dx3x7 

F(4,l)=.5*x7/den;  %dx4xi 

F(4,2)=.5*(-x6)/den; 

F(4,3)=.5*x5/den; 

F(4,4)=  5*f4*(-x4)/den3; 

F(4,5)=.5*(x3/den+f4*(-x5)/den3); 

F(4,6)=.5*(-x2/den-f4*x6/den3); 

F(4,7)=.5*(xl/den+f4*(-x7)/den3); 

F(5,l)=.5*x6/den; 

F(5,2)=.5*x7/den; 

F(5,3)=<5*(-x4)/den; 

F(5,4)=  5*(-x3/den+f5*(-x4)/den3); 
F(5,5)=  5*f5*(-x5)/den3; 
F(5,6)=.5*(xl/den+f5*(-x6)/den3); 
F(5,7)=.5*(x2/den+f5*(-x7)/den3); 

F(6, 1  )=.5*(-x5)/den; 

F(6,2>=.5*x4/den; 

F(6t3)=.5*x7/den; 

F(6,4)=.5*(x2/den+f6*(-x4)/den3); 

F(6,5)=.5*(-xl/den+f6*(-x5)/den3); 

F(6,6)=.5*f6*(-x6)/den3; 

F(6,7)=.5*(x3/den+f6*(-x7)/den3); 

F(7, 1)=.  5*(-x4)/den; 

F(7 ,2)=  .5  *(-x5  )/den; 
F(7,3)=.5*(-x6)/den; 

F(7,4)=.5*(-x  1  /den+f7  *(-x4)/den3); 
F(7,5)=  5*(-x2/den+f7*(-x5)/den3); 
F(7,6)=  5*(-x3/den+f7*(-x6)/den3); 
F(7,7)=.5*f7*(-x7)/den3; 


%dx4x2 
%dx4x3 
%dx4x4 
%dx4x5 
%dx4x6 
%dx4x7 
%dx5xl 
%dx5x2 
%dx5x3 
%dx5x4 
%dx5x5 
%dx5x6 
%dx5x7 
%dx6xl 
%dx6x2 
%dx6x3 
%dx6x4 
%dx6x5 
%d  x6x6 
%dx6x7 
%dx7xl 
%dx7x2 
%dx7x3 
%dx7x4 
%dx7x5 
%dx7x6 
%dx7x7 


%***  Use  first  two  terms  of  infinite  Taylor  Series  expansion  to  form  non-zero  elements  of  Discrete  Fundamental  Matrix,  PHI  *** 


PHI=eye(7)+F*t; 

%*****  Calculate  non-zero  elements  of  Discrete  Process  Noise  Covariance  Matrix  (old  Q)***** 

numl=(.5*TSA2-TSA3/(3*tauomegal));  %convenient  compilation  of  terms  to  be  used  later 

num2==(.5*TSA2-TSA3/(3*tauomega2));  %convenient  compilation  of  terms  to  be  used  later 

num3=(-5*TSA2-TSA3/(3*tauomega3));  %convenient  compilation  of  terms  to  be  used  later 

Q(  1 , 1  )=estSIGOMEGAl  A2*(TS-TSA2/tauomega  1 +TS  A3/ (3  *  tauomega  1 A2)); 
Q(l,4)=estSIGOMEGAlA2*F(4,l)*numl; 

Q(  1 ,5)=estSIGOMEGA  1  A2*F(  1 ,5)*num  1 ; 
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Q(  1 ,6)=estSIG0MEG  A 1 A2  *F(  1 ,6)*num  1 ; 

Q(  1 ,7)=estS  IGOMEG  A 1 A2  *F(  1 ,7)*num  1 ; 

Q(2,2)=estSIGOMEGA2A2*(TS-TSA2/tauoinega2*TSA3/(3*tauomega2)); 

Q(2,4)=estSIGOMEGA2A2*F(4,2)*num2; 

Q(2,5)=estSIGOMEGA2A2*F(5,2)*num2; 

Q(2,6)=estSIGOMEGA2A2*F(6,2)*nuin2; 

Q(2,7)=estSIGOMEGA2A2*F(7,2)*num2; 

Q(3,3)=estSIGOMEGA3A2*(TS-TSA2/tauomega3*TSA3/(3*tauomega3)); 

Q(3,4)=estSIGOMEGA3A2*F(4,3)*num3; 

Q(3,5)=estSlGOMEGA3A2*F(5,3)*num3; 

Q(3,6)=estSIGOMEGA3A2*F(6,3)*num3; 

Q(3,7)=estSIGOMEGA3A2*F(7,3)*nuin3; 

Q(4,1)=Q(1,4); 

Q(4,2)=Q(2,4); 

Q(4,3)=Q(3.4); 

Q(4,4)=estSIGOMEGAlA2*(F(4,l)A2+estSIGOMEGA2A2*F(4,2)A2+estSIGOMEGA3A2*F(4,3)A2)*TSA3/3; 

Q(5,1)=Q(I,5); 

Q(5,2)=Q(2,5); 

Q(5,3)=Q(3,5); 

Q(5,4)=Q(4,5); 

Q(5,5)=estSIGOMEGAlA2*(F(5,l)A2+estSIGOMEGA2A2*F(5,2)A2+estSIGOMEGA3A2*F(5,3)A2)*TSA3/3; 

Q(5,6)=(estSlGOMEGAlA2*F(5,l)*F(6,l)+estSIGOMEGA2A2*F(5,2)*F(6,2)+estSIGOMEGA3A2*F(5,3)*F(6,3))*TSA3/3; 

Q(5,7)=(estSIGOMEGAlA2*F(5,l)*F(7,l)+estSIGOMEGA2A2*F(5,2)*F(7,2)+estSIGOMEGA3A2*F(5,3)*F(7,3))*TSA3/3; 

Q(6,D=Q(1,6); 

Q(6,2)=Q(2,6); 

Q(6,3)=Q(3,6); 

Q(6,4)=Q(4,6); 

Q(6,5)=Q(5,6); 

Q(6,6)=estSIGOMEGAlA2*(F(6,l)A2+estSIGOMEGA2A2*F(6,2)A2+estSIGOMEGA3A2*F(6,3)A2)*TSA3/3; 
Q(6,7)=(estSIGOMEGA  I  A2*F(6, 1  )*F(7, 1  )+estSIGOMEGA2A2*F(6,2)*F(7,2)+estS  IGOMEGA3A2*F(6,3)*F(7,3))*TSA3/3; 
Q(7,l)=Q(l,7); 

Q(7,2)=Q(2,7); 

Q(7,3)=Q(3,7); 

Q(7,4)=Q(4,7); 

Q(7,5)=Q(5,7); 

Q(7,6)=Q(6,7); 

Q(7,7)=estSIGOMEGAlA2*(F(7,l)A2+estSIGOMEGA2A2*F(7,2)A2+estSIGOMEGA3A2*F(7,3)A2)*TSA3/3; 

Q=PH1S*Q; 

%Q=diag([estSIGOMEGAl  estSIGOMEGA2  estSIGOMEGA3  0  000]);  ^constant  values  of  Q...zeros  for  quaternion 

%values  since  these  are  not  noise  driven,  directly  [11] 


%***♦*  Calculate  covariance  before  update,  M  ***** 


PHIT=PHr; 

PHIP=PHI*P; 

PHIPPHrr=PHIP*PHIT; 


%calculate  transpose  of  PHI; 


M=PHIPPHIT+Q;  %calculate  covariance  before  update 

Reusing  old  P,  old  PHI,  and  old  Q) 

t=t+TS;  ^increment  time 

%*****  Propagate  estimated  states  forward  using  one-step  Euler  integration  ***** 

S=0;  Reinitialize  integration  timer 

while  S<=(TS-.0001 )  %loops  from  current  step  to  just  short  of  next  time  step 

x  1  dot=-  1/tauomega  I*xl; 
xl=xl+Hl*xldot; 

x2dot=-  l/tauomega2*x2; 
x2=x2+Hl*x2dot; 

x3dot=-l/tauomega3*x3; 

x3=x3+Hl*x3dot; 

x4dot=.5*f4/den; 
x4=x4+H  1  *x4dot; 

x5dot=.5*f5/den; 

x5=x5+Hl*x5dot; 

x6dot=.5*f6/den; 

x6=x6+Hl*x6dot; 

x7dot=.5*f7/den; 

x7=x7+Hl*x7dot; 

S=S+H1; 
end 

%*****  assign  values  from  numerical  integration  to  arrays  ***** 


%increment  integration  timer  by  defined  integration  step  size 
%  yields  each  state  estimate  integrated  forward  to  next  time  step 


xbar=[xl;x2;x3;x4;x5;x6;x7]; 
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omega  1  bar(count+l  )=x  1 ; 
omega2bar(count+l  )=x2; 
omega3bar(count+l  )=x3; 
qlbar(count+l)=x4; 
q2bar(count+l)=x5; 
q3bar(count+l)=x6; 
q4bar(count+ 1  )=x7 ; 

%*****  Calculate  non-zero  elements  of  the  linearized  measurement  matrix  ***** 

HMAT=eye(7);  %in  this  implementation,  measurements  are  the  states 

%*****  Calculate  Measurement  Noise  Matrix,  RMAT  ***** 

%*****  Uses  A  matrix  from  Gauss„newton  to  relate  6x6  RSENSOR  to  covariance  of  quaternions  after  convergence  [i  l]  ****** 
RMAT=[ROMEGA,  zeros(3,4);zeros(4,3),  A(:,:,count)*RSENSOR*A(:,:,count)']; 

%*****  Calculate  Kalman  gain  matrix  ***** 


HT=HMAT; 


^calculate  transpose  of  HT  (next  time  step) 


HM=HMAT*M; 
HMHT=HM*HT; 
HMHTR=HMHT+RMAT ; 
HMHTRINV=inv(HMHTR); 
MHT=M*HT; 


GAIN=MHT*HMHTRINV;  ^Calculate  Kalman  gain  matrix 

KH=GAIN*HMAT; 

KH=eye(statesize)-KH; 

%*****  Calculate  covariance  matrix  after  update,  P  ***** 

P=IKH*M;  ^Calculate  covariance  after  update  (new  P) 

%*****  Calculate  residuals:  actual  new  measurement  -  projected  new  measurement  ***** 

%*****  Since  the  H  matrix  is  the  identity  matrix  in  this  implementation,  the  "zbar",  ***** 

%*****  or  projected  measurement,  is  simply  the  projected  state  (zbar=xbar)!  ***** 


omega  1  res=(omega  1  _q_min(count+ 1  )-x  1 ); 
omega2res=(omega2_q_min(count+ 1  )-x2) ; 
omega3res=(omega3_q_min(count+ 1  )-x3); 


%calculate  residual  from  propagated  state  and  measurement 
%calculate  residual  from  propagated  state  and  measurement 
%calculate  residual  from  propagated  state  and  measurement 
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q  i  res=q_min(  l  ,count+ 1  )-x4; 
q2res=q_min(2,count+l  )-x5; 
q3res=q_min(3,count+l  )-x6; 
q4res=q_min(4,count+l)-x7; 


^calculate  residual  from  propagated  state  and  measurement 
%calculate  residual  from  propagated  state  and  measurement 
^calculate  residual  from  propagated  state  and  measurement 
%calculate  residual  from  propagated  state  and  measurement 


residual=[omegalres;omega2res;omega3res;qlres;q2res;q3res;q4res];  %form  vector  of  residuals 


%*****  Calculate  updated  state  estimates  using  propagated  states  and  Kalman  gains 


xhat=xbar+GAIN*residual; 


%*****  Check  for  loss  of  data  ***** 


if  q_min(l  :4,count)=[0  0  0  1]*; 
omega  1  hat(count+ 1  )=omega  1  hat(count); 
omega2hat(count+l)=omega2hat(count); 
omega3hat(count+ 1  )=omega3hat(count); 
q  1  hat(count+ 1  )=q  1  hat(count); 
q2hat(count+l  )=q2hat(count); 
q3hat(count+ 1  )=q3hat(count); 
q4hat(count+ 1  )=q4hat(count); 
flag(count)=l; 

else 

%***************  Data  management  ***************** 

omega  1  hat(count+l  )=xhat(l ); 
omega2hat(count+l  )=xhat(2); 
omega3hat(count+l  )=xhat(3); 
qlhat(count+ 1  )=xhat(4); 
q2hat(count+ 1  )=xhat(5); 
q3hat(count+ 1  )=xhat(6); 
q4hat(count+ 1  )=xhat(7); 
flag(count)=0; 

end 

ArrayT(count+l)=t; 

%♦*** ***********  prror  Analysis  ****************** 

SP1 1  =sqrt(P(  1,1)); 

SP11P=-SP11; 

SP22=sqrt(P(2,2)); 

SP22P=-SP22; 


%  Calculate  theoretical  upper  error  bound  on  omega  1  hat 
^Calculate  theoretical  lower  error  bound  on  omega  1  hat 
^Calculate  theoretical  upper  error  bound  on  omega2hat 
5Calculate  theoretical  lower  error  bound  on  omega2hat 
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SP33=sqrt(P(3,3)); 

SP33P=-SP33; 

SP44=sqrt(P(4,4)); 

SP44P=-SP44; 

SP55=sqrt(P(5,5)); 

SP55P=-SP55; 

SP66=sqrt(P(6,6)); 

SP66P=-SP66; 

SP77=sqrt(P(7,7)); 

SP77P=-SP77; 

ArraySPI  l(count+l)=SPl  1; 
ArraySPl  lP(count+l)=SPl  IP; 
ArrayS  P22(count+l  )=SP22; 
ArrayS  P22P(count+l  )=SP22P; 
ArrayS  P33(count+l  )=SP33; 
ArraySP33P(count+ 1  )=SP33P; 
ArraySP44(counH-l  )=SP44; 
ArraySP44P(count+ 1  )=SP44P; 
ArraySP55(count+l)=SP55; 
ArraySP55P(count+ 1  )=SP55P; 
ArraySP66(count+l  )=SP66; 
ArraySP66P(count+l)=SP66P; 
ArrayS  P77(count+l  )=SP77; 
ArraySP77P(count+l)=SP77P; 

count=count+l; 

end 


%Calculate  theoretical  upper  error  bound  on  omega3hat 
%Calculate  theoretical  lower  error  bound  on  omega3hat 
^Calculate  theoretical  upper  error  bound  on  qlhat 

^Calculate  theoretical  lower  error  bound  on  qlhat 
^Calculate  theoretical  upper  error  bound  on  q2hat 
%Calculate  theoretical  lower  error  bound  on  q2hat 
^Calculate  theoretical  upper  error  bound  on  q3hat 
%  Calculate  theoretical  lower  error  bound  on  q3hat 
^Calculate  theoretical  upper  error  bound  on  q4hat 
%  Calculate  theoretical  lower  error  bound  on  q4hat 


%  increment  counter 


%******  cajcuiate  mean  square  errors  for  "instantaneous"  and  for  EKF  ****** 

<%,******  mean  square  error  excludes  first  5  samples  to  reduce  effects  of  transients  ***** 


s2=length(omega  l„q_min); 
sl=5; 


omega  l_q_min_mse=mean_square_error(omegal_q_min(sl  :s2),omegal(sl  :s2)); 
omega2_q_min_mse=mean_square_error(omega2_q__min(sl  :s2),omega2(s  1  :s2)); 
omega3_q_min_mse=mean_square_error(omega3_q_min(sl  :s2),omega3(s  1  :s2)); 
q  l_q_min_mse=mean_square_error(q_min(  1  ,(s  l  :s2)),q  1  (s  1  :s2)); 
q2_q_min_mse=mean_square_error(q_min(2,(s  1  :s2)),q2(s  1  :s2)); 
q3_q_min_mse=mean_square_error(q_min(3,(s  1  :s2)),q3(s  1  :s2)); 
q4_q_min_mse=mean_square_error(q„min(4,(s  1  :s2)),q4(s  1  :s2)); 


omega  l_ekf_mse=mean_square_error(o  mega  1  hat(sl  :s2),omegal(sl  :s2)); 
omega2_ekf_mse=mean_square_error(omega2hat(s  1  :$2),omega2(s  1  :s2)); 
omega3_ekf_mse=mean_square„error(omega3hat(s  1  :s2),omega3(s  l:s2)); 
q  l_ekf _mse=mean_sq  uare_error(q  1  hat(s  1  :s2),q  1  (s  1 :  s2)) ; 
q2_ekLmse=mean_square_error(q2hat(sl  :s2),q2(s  1  :s2)); 
q3_ekf_mse=mean_square_error(q3hat(s  1  ;s2),q3(sl  :s2)); 
q4_ekfjrnse=mean_square_error(q4hat(s  1  :s2),q4(s  1  :s2)); 

%***************  plotting  and  output  statements  ***************** 

plots=input('Enter  "1"  to  display  plots.  Enter  "0"  for  no  plots.  Default  is  no  plots:  '); 
if  isempty(plots) 
plots=0; 
end 

if  plots=l 

%******  plots  of  error  vs.  true  and  theoretical  error  bounds  ******* 

plot(  Array  T,  (omega  1  ( 1  :s2)-omega  1  hat),  ArrayT,  ArrayS  P 1 1  ,'x\  ArrayT,  ArraySP  1 1  P,'x'); 

grid  on;title(*enror  in  EKF  estimate  of  omega  T); 

legend(’error  in  omega  1  hat’, ’theoretical  bound'); 

xlabel('time  (sec)');ylabel('error  in  omega  that  (rad/s)’); 

pause;close;clc; 

plot(ArrayT,(omega2(l  :s2)-omega2hat),  Array  T,ArraySP22, ’x1,  ArrayT, AiTaySP22P,'x,); 

grid  on;title('error  in  EKF  estimate  of  omega2’); 

legend('error  in  omega2hat’,,theoretical  bound’); 

xlabel('time  (sec),);ylabel(’error  in  omega2hat  (rad/s)’); 

pause;close;clc; 

plot(  Array  T,(omega3(  1  :s2)-omega3hat),  ArrayT,  ArrayS  P33,’x*,  ArrayT,  ArraySPSSP/x*); 

grid  on;tit!e('error  in  EKF  estimate  of  omega3'); 

legend('error  in  omega3hat,,,theoretical  bound'); 

xlabel(’time  (sec)');yIabel('error  in  omega3hat  (rad/s)*); 

pause;close;clc; 

plot(ArrayT,(q  1  (1  :s2)-qlhat),  ArrayT,  ArraySP44,,x',  ArrayT,  ArrayS  P44P,'x’); 

grid  on;title('error  in  EKF  estimate  of  ql*); 

legend('error  in  ql  hat*, 'theoretical  bound'); 

xlabel(’time  (sec),);ylabel(’error  in  ql'); 

pause;close;c!c; 


plot(ArrayT,(q2(l  :s2)-q2hat),  ArrayT,  ArraySP55,'x',  ArrayT,  ArraySP55P,'x'); 


grid  on;title('error  in  EKF  estimate  of  q2’); 
legend('error  in  q2hat','theoretical  bound'); 
xlabel(’time  (sec)');ylabel('error  in  q2’>; 
pause;close;clc; 

plot(ArrayT,(q3(l:s2)-q3hat),ArrayT,ArraySP66,'x',ArrayT,ArraySP66P,’x'); 

grid  on;title('error  in  EKF  estimate  of  q3’); 

legend('error  in  q3hat', 'theoretical  bound'); 

xlabel(*time  (sec)’);yIabeI('error  in  q3*); 

pause;close;clc; 

plot(ArrayT,(q4(l:s2)-q4hat)1  ArrayT,  ArraySP77,'x',  ArrayT,  ArrayS  P77P,’x'); 

grid  on;title(’error  in  EKF  estimate  of  q4'); 

legend('error  in  q4hat’, 'theoretical  bound'); 

xlabel('time  (sec)’);ylabel('error  in  q4'); 

pause;close;clc; 

9fc******  plots  to  show  true  vs.  quaternion  derived  alone  vs.  EKF  estimates  ******* 

plot(  ArrayT,  omega l_q_min,'gx',  ArrayT,  omega  1  hat, *m',  ArrayT,  omegal(l:s2),,-');grid  on; 
titleCQuatemion  derived  omega  1,  EKF  estimate  of  omega  1,  "True"  omega  1' ); 
xlabel('time  (sec)');ylabel('omegal  (rad/s)'); 

legend('omegal  from  differencing  quaternions', 'omega  1  from  EKF, 'true  omega  1'); 
pause;clc;close; 

plot(  ArrayT,  omega2_q_min,'gx\ ArrayT,  omega2hat,’m',  ArrayT,  omega2(l:s2),’--');grid  on; 
titleCQuatemion  derived  omega2,  EKF  estimate  of  omega2,  "True"  omega2’ ); 
xlabel('time  (sec)’);ylabelComega2  (rad/s)'); 

legend(*omega2  from  differencing  quaternions', 'omega2  from  EKF,'true  omega2'); 
pause;clc;close; 

plot(  ArrayT,  omega3„q_min,'gx',  ArrayT,  omega3hat,'m',  ArrayT,  omega3(l:s2),'--');grid  on; 
titleCQuatemion  derived  omega3,  EKF  estimate  of  omega3,  "True"  omega3* ); 
xlabel('time  (sec)');ylabelComega3  (rad/s)'); 

legend('omega3  from  differencing  quatemions’,'omega3  from  EKF,'true  omega3'); 
pause;clc;close; 

end 

'Summary  of  performance  of  each  method.,.' 

’The  MSE  in  omega l  derived  from  differencing  quaternions  is  (rad/s):  V  \omegal_q_min_mse 


'The  MSE  in  omega  1  for  the  EKF  estimates  is  (rad/s):  7  ’,omegal_ekf_mse 
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‘The  MSE  in  omega2  derived  from  differencing  quaternions  is  (rad/s):  7  \omega2_q_min_mse 
The  MSE  in  omega2  for  the  EKF  estimates  is  (rad/s):  7  \omega2„ekf„mse 

«  f 

’Please  press  “return”  to  continue...* 
pause;clc;cIose; 

’The  MSE  in  omega3  derived  from  differencing  quaternions  is  (rad/s):  7  \omega3_q_min_mse 

« t 

‘The  MSE  in  omega3  for  the  EKF  estimates  is  (rad/s):  7  ',omega3„ekf„mse 

’The  MSE  in  ql  derived  using  Gauss-Newton  is:  7  \ql_q_min_mse 

The  MSE  in  ql  for  the  EKF  estimates  is:  7  \ql_ekf_mse 

’Please  press  "return”  to  continue...' 
pause;clc;close; 

’Summary  of  performance  of  each  method  (cont.) ...’ 

f  l 

'The  MSE  in  q2  derived  using  Gauss-Newton  is:  7  \q2_q_min_mse 

The  MSE  in  q2  for  the  EKF  estimates  is: 7  \q2_ekf_mse 

'press  "return”  to  continue...' 
pause  ;clc;close; 

'Summary  of  performance  of  each  method  (cont.) ...’ 

'The  MSE  in  q3  derived  using  Gauss-Newton  is:  7  \q3_q_min_mse 

The  MSE  in  q3  for  the  EKF  estimates  is: \q3_ekf_mse 

'The  MSE  in  q4  derived  using  Gauss-Newton  is: ','  \q4„q_min_mse 

'The  MSE  in  q4  for  the  EKF  estimates  is:  7  \q4_ekf_mse 

'press  "return”  to  continue...' 
pause  ;clc;c!ose; 

'Summary  of  performance  of  each  method  (cont.) 


%******  rotate  each  vector  using  actual  and  ekf  attitude  quaternion  ****** 


%******  check  mean-square-error  between  vectors  ****** 


*******  Evaluate  Mean-Square-Error  of  rotated  vectors  using  Gauss-Newton  derived  quaternion  ******* 
'Press  "return"  to  rotate  the  reference  vector  at  each  time  step  to  the' 

'representation  in  the  rotated  frame  using  both  the  actual  rotation  quaternion' 

'and  the  quaternion  derived  using  the  ekf.. .once  rotation  is  complete,' 

'calculate  the  mean-square-error  between  the  results.' 
pause;close;clc; 

qekf=[q  1  hat;q2hat;q3hat;q4hat] ;  %matrix  representation  of  ekf  quaternions 

ekf_mse„error=[  ]; 
for  j=l:count; 

ekfvec(  1 :6,j)=qframerot6(qekf(  1 :4j),vref(  1 :6,j)); 

Q_vec(l  :6  j)=qframerot6(Qvec(l:4j),vref(l  :6,j)); 
ekf_mse_error(j)=mean_square_error(ekfvec(  1 :6,j),Q„vec(l 
end 

%*****  determine  angle  between  rotated  vectors  ***** 
for  k-l:count 

ekfsunangle(k)=acos(dot(ekfvec(  1 :3,k),Q_vec(  1 :3,k))/(norm(ekfvec(  1 :3,k))*norm(Q_vec(l  :3,k)))); 
ekfmagang!e(k)=acos(dot(ekfvec(4:6,k),Q_vec(4:6,k))/(norm(ekfvec(4:6,k))*norm(Q_vec(4:6,k)))); 
GNsunangle(k)=acos(dot(qjnnnvec(l:3,k),Q„vec(l:3,k))/(norm(q_minvec(l:3,k))*norm(Q_vec(l:3,k)))); 
GNmagangle(k)=acos(dot(q_minvec(4:6,k),Q_vec(4:6,k))/(norm(q_minvec(4:6,k))*norm(Q_vec(4:6,k)))); 
end 

ekfsunangle=ekfsunangle*  1 80/pi; 
ekfmagangle=ekfmagangle*  1 80/pi; 

GNsunangle-GNsunangle*  1 80/pi; 

GNmagangIe=GNmagangle*  1 80/pi; 

%******  Plot  vectors  rotated  using  derived  and  actual  quaternions  ***** 

%plot3(Q_vec(l,l  :count),Q_vec(2,l  :count),Q_vec(3,l  :count),*b');grid  on;ho!d  on; 

%plot3(ekfvec(  1 , 1  :count),ekfvec(2, 1  :count),ekfvec(3, 1  :count),'m'); 

%plot3(Q_vec(4,l  :count),Q_vec(5,l:count),Q_vec(6,l  :count),'r’);grid  on;hold  on; 

%plot3(ekfvec(4, 1  :count),ekfvec(5, 1  :count),ekfvec(6, 1  :count),'g'); 

%xlabel('X  component’^ylabeK'Y  component'^zlabelOZ  component'); 

%!egend(’sun  vector,  actual  q',’sun  vector,  ekf  q','mag  field  vector,  actual  q',  'mag  field  vector,  ekf  q’); 
%tit!e('3-D  plot  of  reference  vectors  rotated  using  actual  quaternions  and  ekf  derived  quaternions’); 
%pause;close;clc; 

%******  Plot  mean-square-error  between  reference  vector  rotated  using  actual  quaternion  ****** 

%******  and  reference  vector  rotated  using  derived  quaternion.  ****** 


%rotate  ref  vector  using  min  error  quaternion 
%rotate  ref  vector  using  true  quaternion 
:6 j));  %calculate  MSE  between  rotated  vectors 


%plot(T(  l  :count),ekf_mse_error(l  :count)),grid  on; 


%title('mean-square  error  between  reference  vectors  rotated  using  ekf  derived  and  actual  quaternions'); 
%xlabel('time  (secy^ylabeK'MSE'^legendOMSE') 

%'press  "return"  to  continue...' 

%pause;c!ose;clc; 


'*********************  Overall  Performance  Indicator  ************************* 

*******  Excludes  first  five  measurements  to  reduce  effect  of  transients  ******* 

'The  average  of  the  omega  mean -square-errors  for  the  differencing  method  is:  ' 
GNomegaperf=(omega  1  _q_min_mse+omega2_q_min_mse+omega3_q_min_mse)/3 

The  average  of  the  omega  mean-square-errors  for  the  ekf  results  is:  ' 
ekfomegaperf=(omega  1  _ekf_mse+omega2_ekf_mse+omega3_ekf_mse)/3 

'The  average  of  the  quaternion  component  mean-square-errors  for  the  Gauss-Newton  method  is:  * 
GNqperf=(ql_q_min_mse+q2_q_min_mse+q3_q_min_mse+q4_q_min_mse)/4 

’The  average  of  the  quaternion  component  mean-square -errors  for  the  ekf  results  is:  ' 
ekfqperf=(q  1  _ekf_mse+q2_ekf_mse+q3_ekf_mse+q4_ekf_mse)/4 

'press  "return"  to  continue...’ 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) 

'The  average  MSE  for  the  ref  vectors  rotated  using  GN  quaternions:  ' 
mean_GN_mse_error=mean(GN_mse_error) 

'The  average  MSE  for  the  ref  vectors  rotated  using  ekf  quaternions:  ' 
mean_ekf_mse_error=mean(ekf_mse_error) 

'press  "return"  to  continue...’ 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) ...' 

'overall  GN  performance:  sqrt((mean  omega  MSE)A2  +  (mean  rotated  vector  MSE)A2)]' 
overaII_GN_perf=sqrt(GNomegaperfA2+mean_GN_mse_errorA2) 

'overall  ekf  performance:  sqrt((mean  omega  MSE)A2  +  (mean  rotated  vector  MSE)A2)]' 
overaU_ekf_perf=sqrt(ekfomegaperfA2+mean_ekf_mse_errorA2) 


%»***************************************************************************************• 
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^•*********** ************ **********  ekf summary  **************************************** 
%************ ************************************************************ ***************** 
’omega:  \ekfomegaperf 

'quaternion:  \mean_ekf_mse_error 
’overall:  \overall_ekf_perf 
’mean  sun  angle  error:  \mean(ekfsunangle) 

'std  sun  angle  error:  \std(ekfsunangle) 

'max  sun  angle  error:  \max(ekfsunangle) 

'mean  mag  angle  error  \mean(ekfmagangle) 

'std  mag  angle  error:  \std(ekfmagangle) 

’max  mag  angle  error  \max(ekfmagangle) 

'press  "return*’  to  continue...' 
pause;close;clc; 

angles=input('Enter  "1”  to  calculate  attitude  error  angles.  Enter  "0"  for  no  angles.  Default  is  no  angles:  ’); 
if  isempty(angles) 
angles=0; 
end 


if  angles==  1 


^Calculate  angles  between  body  frame  rotated  using  estimated  quaternion  and 
%rotated  using  "true”  quaternion 

[ovec  1  ,ovec2,ovec3,ovec  1  hat,ovec2hat,ovec3hat,angl  ,ang2,ang3]=orientation3(Qvec,  qekf); 
end 

%end  of  program 

A.  14  Norates_ukf.m 

Norates_ukf.m  is  the  MATLAB  script  that  implements  the  unscented  Kalman  filter  (UKF)  designed  in 
Chapter  9.  This  implementation  is  for  the  case  where  rotational  rates  are  not  directly  measured,  but  are 
derived  from  quaternion  rates.  Norates_ukf.m  is  called  by  attitude_filter.m  if  the  appropriate  option  is 
selected  by  the  user. 

%NORATES_UKF  -  Discrete  Unscented  Kalman  Filter  Implementation 
%Uses  data  generated  by  attitude_sim.m  to  estimate  3-D  body  rates  and 
%the  attitude  quaternion  of  a  sounding  rocket  given  noisy  measurements 
%from  two  attitude  sensors.  Uses  a  Gauss-Newton  optimization  and  an 


%Unscented  Kalman  Filter  (UKF)  to  generate  a  best  estimate. 
% 

%Written  by  Mark  Charlton.  Last  modified  28  October  03. 
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%Object  subject  to  rotational  motion  -  non-linear  Equation  of  Motion 
%*****  Initialize  parameters  and  matrices  ***** 

clc 


ics=[  ]; 
sigmas=[  ]; 
error=[  ]; 
sensorerror=[  ]; 

ArrayT=[  ]; 
qlhat=[  ]; 
q2hat=[  ]; 
q3hat=[  ]; 
q4hat=[  ]; 
omega  lhat=[]; 
omega2hat=[  ]; 
omega3hat=[  ]; 

ArraySPl  1=[  ]; 

Array  SP11P=[  ]; 

ArraySP22=[  ]; 

Array  SP22P=[  ]; 

ArraySP33=[  ]; 

Array  SP33P=[  ]; 

ArrayS  P44=[  ]; 

ArraySP44P=[  ]; 

ArraySP55=[  ]; 

ArrayS  P55P=[  ]; 

ArraySP66=[  ]; 

Array  SP66P=[  ]; 

Array  SP77=[  ]; 

ArraySP77P=[  ]; 
statesize=7; 

PHIS=0; 
dynamic  model) 
Q=zeros(statesize,statesize); 
P-zeros(statesize,statesize); 
HMAT=zeros(statesize, states  ize) ; 


%  initialize  error  multiplier 
^initialize  error  multiplier 
^initialize  Arrays 


%  system  order 

%process  noise  (measure  of  uncertainty  in 

%  initialize  process  noise  matrix 
^initialize  covariance  matrix 
%  initialize  linearized  measurement  matrix 


%*****  Assign  initial  estimates  for  omegalhat,  omega2hat,  omega3hat,  qlhat,  q2hat,  q3hat,  q4hat  ***** 


'Do  you  wish  to  enter  initial  estimates  of  the  rotational  body  rates  and  the  quaternion  components' 
'or  let  the  program  use  the  actual  initial  conditions  with  a  user  defined  percent  error  to' 

’make  an  initial  estimate  (default  is  to  let  the  program  do  it)?* 

'If  you  choose  manual  entry,  an  uncertainty  in  the  estimate  is  also  required.  The  default  is  "auto".' 

ics=input('Enter  "1"  for  manual  entry  and  "0"  for  automatic:  '); 
if  isempty(ics) 
ics=0; 
end 


if  ics  =  0 

%*****  base  initialization  on  actual  intial  conditions  with  user  defined  standard  error  ***** 
clc 

error=input('Enter  percentage  error  (+/-)  in  initial  conditions  (i.e.,  -5.2%  =  "-5.2”)  Default  is  5%:  '); 
if  isempty(error) 
error=5; 
end 

'percent  error  applied  to  the  initial  conditions  is:  \error 
error=error/l00; 

omega  1  hat(l  )=omega  1  ( 1  )+omega  1  ( l  )*error; 

omega2hat(  1  )=omega2(  1  )+omega2(  1  )*error; 

omega3hat(  1  )=omega3  ( 1  )+omega3  ( 1  )*error; 

qlhat(l)=ql(l)+ql(l)*error; 

q2hat(  1  )=q2(  1  Hq2(  1  )*error; 

q3hat(  1  )=q3(  1  Hq3(  1  )*error; 

q4hat(  1  )=q4(  1  )+q4(  1  )*error; 


'apriori  estimate  of  omega  1  (rad/s):  \omegalhat(l) 

%apriori  estimate  of  omega  1 

'apriori  estimate  of  omega2  (rad/s):  ’,omega2hat(l) 

%apriori  estimate  of  omega2 

'apriori  estimate  of  omega3  (rad/s):  \omega3hat(l) 

%apriori  estimate  of  omega3 

'apriori  estimate  of  ql :  \qlhat(l) 

%apriori  estimate  of  ql 

'apriori  estimate  of  q2:  ',q2hat(l) 

%apriori  estimate  of  q2 

'apriori  estimate  of  q3:  ',q3hat(l) 

%apriori  estimate  of  q3 

'apriori  estimate  of  q4:  ’,q4hat(l) 

%apriori  estimate  of  q4 

%*****  Non-zero  entries  of  initial  covariance  matrix  for  auto  initial  cond  ***** 
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P(  1 , 1  )=(omegal  ( 1  )*error+eps)A2; 
P(2,2)=(omega2(  1  )*error+eps)  A2; 
P(3,3)=(omega3(  1  )*error+eps)A2; 
p(4,4)=(ql(  1  )*error+eps)A2; 
P(5,5)=(q2(l)*error+eps)A2; 
P(6,6)=(q3(  1  )*error+eps)A2; 
P(7»7)=(q4(  1  )*error+eps)A2; 


%set  at  square  of  uncertainty  in  omega  1 
%set  at  square  of  uncertainty  in  omega2 
%set  at  square  of  uncertainty  in  omega3 
%set  at  square  of  uncertainty  in  ql 
%set  at  square  of  uncertainty  in  q2 
%set  at  square  of  uncertainty  in  q3 
%set  at  square  of  uncertainty  in  q4 


else 


%...or  can  manually  enter  whatever  intialization  values  you  wish... 


omega lhat(l)=input(’Please  enter  initial  omegal  in  rpm:  '); 
omega2hat(l)=input(‘PIease  enter  initial  omega2  in  rpm:  '); 
omega3hat(l)=input('Please  enter  initial  omega3  in  rpm:  '); 
qlhatOHnputf’Please  enter  initial  ql:  ’); 
q2hat(l)=input('Please  enter  initial  q2:  '); 
q3hat(l)=input('Please  enter  initial  q3:  '); 
q4hat(l)=input('Please  enter  initial  q4:  ’); 


%apriori  estimate  of  omegal 
%apriori  estimate  of  omega2 
%apriori  estimate  of  omega3 
%apriori  estimate  of  ql 
%apriori  estimate  of  q2 
%apriori  estimate  of  q3 
%apriori  estimate  of  q4 


omega  1  hat(  1  )=omega  1  hat(  1  )*2*pi/60; 
omega2hat(l  )=omega2hat(  I  )*2*pi/60; 

omega3hat(  1  )=omega3hat(  I  )*2*pi/60;  ^convert  manual  omega  entries  to  rad/s 


'For  manual  entry  of  initial  conditions,  estimated  uncertainty  in  the  initial  conditions' 
'is  necessary  to  intialize  the  covariance  matrix...' 


domegalhat=input('PIease  enter  estimated  uncertainty  in  initial  omegal  (rad/s):  '); 
domega2hat=input('PIease  enter  estimated  uncertainty  in  initial  omega2  (rad/s):  '); 
domega3hat=input('PIease  enter  estimated  uncertainty  in  initial  omega3  (rad/s):  '); 
dql  hat=input('Please  enter  estimated  uncertainty  in  initial  ql :  '); 
dq2hat=input('Please  enter  estimated  uncertainty  in  initial  q2:  '); 
dq3hat=input(’Please  enter  estimated  uncertainty  in  initial  q3:  *); 
dq4hat=input('Please  enter  estimated  uncertainty  in  initial  q4: 


%  uncertainty  in  initial  estimate  of  omegal 
%uncertainty  in  initial  estimate  of  omega2 
%  uncertainty  in  initial  estimate  of  omega3 
%uncertainty  in  initial  estimate  of  ql 
^uncertainty  in  initial  estimate  of  q2 
%  uncertainty  in  initial  estimate  of  q3 
%  uncertainty  in  initial  estimate  of  q4 


%*****  Non-zero  entries  of  initial  covariance  matrix  for  manual  initial  cond  ***** 


P(  1 , 1  )=(domega  1  hat+eps)  A2; 

P(2,2)=(domega2hat+eps)A2; 

P(3,3)=(domega3hat+eps)A2; 

P(4,4)=(dq  1  hat+eps)A2; 

P(5,5)=(dq2hat+eps)A2; 

P(6,6)=(dq3hat+eps)A2; 

P(7,7)=(dq4hat+eps)A2; 


%set  at  square  of  uncertainty  in  initial  estimate  of  omegal 
%set  at  square  of  uncertainty  in  initial  estimate  of  omega2 
%set  at  square  of  uncertainty  in  initial  estimate  of  omega3 
%set  at  square  of  uncertainty  in  initial  estimate  of  ql 
%set  at  square  of  uncertainty  in  initial  estimate  of  q2 
%set  at  square  of  uncertainty  in  initial  estimate  of  q3 
%set  at  square  of  uncertainty  in  initial  estimate  of  q4 
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end 

%insure  initial  quaternion  estimate  is  a  unit  quaternion 

mag=sqrt(ql  hat(  1 )  A2+q2hat(  1  )A2+q3hat(  1  )A2+q4hat(  1  )A2); 

q  1  hat(  1  )=q  1  hat(  1  )/mag; 

q2hat(  l  )=q2hat(  1  )/mag; 

q3hat(  l  )=q3hat(l  )/mag; 

q4hat(  1  )=q4hat(l  )/mag; 

%*****  Enter  measurement  noise  values  for  use  in  calculating  Measurement  Noise  Matrix,  R  ***** 

'Do  you  wish  to  enter  sensor  measurement  sigmas  manually  or  let  the  program  use  the  actual' 

'sensor  measurement  sigmas  with  a  user  defined  percent  error  to  make  an  initial  estimate* 

'(default  is  to  let  the  program  do  it)?' 

sigmas=input('Enter  "1”  for  manual  entry  and  "0"  for  automatic:  '); 
if  isempty(sigmas) 
sigmas=0; 
end 
clc 

if  sigmas  =  0 

%*****  base  sensor  noise  matrix  on  actual  sigmas  plus  user  defined  error  ***** 

sensorerror=input('Enter  percentage  error  (+/-)  in  sensor  measurement  sigmas  (i.e.,  -5.2%  =  "-5.2")  Default  is  0%:  '); 
if  isempty(sensorerror) 
sensorerron=0; 
end 

'percent  error  applied  to  the  actual  sensor  measurement  sigmas  is:  \sensorerror 
sensorerror=sensorerror/ 1 00; 

SIGOMEGA1  =stdomegal ;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

S  IGOMEG  A2=stdomega2;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

SIGOMEGA3=stdomega3;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

estS  IGOMEG  A1 =SIGOMEG  A 1 +S IGOMEG  A 1  *sensorerror; 

estSIGOMEGA2=SIGOMEGA2+SIGOMEGA2*sensorerror; 

estS  IGOMEG  A3=S  IGOMEG  A3+S  IGOMEG  A3  *sensorerror; 

estSIGSUNX=SIGSUNX+SIGSUNX*sensorerror; 

estSIGSUNY=SIGSUNY+SIGSUNY*sensorerror; 

estSIGSUNZ=SIGSUNZ+SIGSUNZ*sensorerror; 


estSIGMAGX=SIGMAGX+SIGMAGX*sensorerror; 
estSIGMAGY=SIGMAGY+SIGMAGY*sensorerror, 
estS  IGM  AGZ=S  IGM  AGZ+S IGM  AGZ*sensorerror ; 
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'apriori  estimate  of  omega  1  "measurement”  sigma  (rad/s):  \estSIGOMEGAl 
’apriori  estimate  of  omega2  "measurement”  sigma  (rad/s):  \estSIGOMEGA2 
’apriori  estimate  of  omega3  "measurement"  sigma  (rad/s):  ',estSIGOMEGA3 
’apriori  estimate  of  sun  sensor  x  axis  measurement  sigma  (deg):  \estSIGSUNX*  180/pi 
'apriori  estimate  of  sun  sensor  y  axis  measurement  sigma  (deg):  ’.estSIGSUNY*  180/pi 
’apriori  estimate  of  sun  sensor  z  axis  measurement  sigma  (deg):  \estSIGSUNZ*  180/pi 
'apriori  estimate  of  mag  field  sensor  x  axis  measurement  sigma  (deg):  \estSIGMAGX*  180/pi 
'apriori  estimate  of  mag  field  sensory  axis  measurement  sigma  (deg):  \estSIGMAGY*  180/pi 


%apriori  estimate  of  SIGOMEGA1 
%apriori  estimate  of  SIGOMEGA2 
%apriori  estimate  of  SIGOMEGA3 
%apriori  estimate  of  SIGSUNX 
%apriori  estimate  of  SIGSUNY 
%apriori  estimate  of  SIGSUNZ 
%apriori  estimate  of  SIGMAGX 
%apriori  estimate  of  SIGMAGY 


'apriori  estimate  of  mag  field  sensor  z  axis  measurement  sigma  (deg):  \estSIGMAGZ*  180/pi  %apriori  estimate  of  SIGMAGZ 


else 


%...or  can  manually  enter  whatever  intialization  values  you  wish... 


estSIGOMEGAl=input('Please  enter  sigma  for  the  omegal  derived  "measurement”  in  rpm:  ');  %apriori  estimate  of  SIGOMEGA1 
estSIGOMEGA2=input(*PIease  enter  sigma  for  the  omega2  derived  "measurement"  in  rpm:  ’);  %apriori  estimate  of  SIGOMEGA2 
estSIGOMEGA3=input('Please  enter  sigma  for  the  omega3  derived  "measurement"  in  qpm:  ');  %apriori  estimate  of  SIGOMEGA3 


estSIGOMEGA  1  =estSIGOMEGAl  *2*pi/60; 
estSIGOMEGA2=estSIGOMEGA2*2*pi/60; 
estSIGOMEGA3=estSIGOMEGA3*2*pi/60; 


^convert  estSIGOMEGA  1  to  rad/s 
^convert  estSIGOMEGA2  to  rad/s 
^convert  estSIGOMEGA3  to  rad/s 


estSIGSUNX=input('Please  enter  sigma  for  the  sun  sensor  x  axis  measurement  in  deg:  '); 
estSIGSUNY=input('Please  enter  sigma  for  the  sun  sensor  y  axis  measurement  in  deg:  ’); 
estSIGSUNZ=input('Please  enter  sigma  for  the  sun  sensor  z  axis  measurement  in  deg:  '); 
estSIGMAGX=input(’Please  enter  sigma  for  the  mag  field  sensor  x  axis  measurement  in  deg:  ’); 
estSIGMAGY=input('Please  enter  sigma  for  the  mag  field  sensor  y  axis  measurement  in  deg:  '); 
estS  IGM  AGZ=input('Please  enter  sigma  for  the  mag  field  sensor  z  axis  measurement  in  deg:  '); 


%apriori  estimate  of  SIGSUNX 
%apriori  estimate  of  SIGSUNY 
%apriori  estimate  of  SIGSUNZ 
%apriori  estimate  of  SIGMAGX 
%apriori  estimate  of  SIGMAGY 
%apriori  estimate  of  SIGMAGZ 


estSIGSUNX=estSIGSUNX*pi/180; 
estSIGSUNY=estSIGSUNY*pi/180; 
estSIGSUNZ=estSIGSUNZ*pi/180; 
estS  IGM  AGX=estS  IGM  AGX*pi/ 1 80; 
estS  IGM  AG  Y=estSIGM  AGY*pi/ 1 80; 
estSIGMAGZ=estSIGMAGZ*pi/180; 


%convert  from  degrees  to  radians 
^convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 
Reconvert  from  degrees  to  radians 
%convert  from  degrees  to  radians 


end 


'Please  press  "return"  to  continue...’ 
pause;clc;c!ose; 
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%*****  Non-zero  submatrices  of  Measurement  Noise  Matrix,  R  ***** 

ROMEGA=diag([estSIGOMEG A 1  A2+eps  estSlGOMEGA2A2+eps  estSIGOMEGA3A2+eps]); 

RSENSOR=diag([estSIGSUNXA2+eps  estSIGSUNYA2+eps  estSIGSUNZA2+eps  estSIGMAGXA24eps  estS IGM AG YA2+eps 
estSIGMAGZA2+eps]); 

%*****  user  input  of  process  noise  to  reflect  uncertainty  in  dynamic  model,  PHIS  ***** 

«  l 

confidence=input('Enter  process  noise  to  reflect  uncertainty  in  model  (default  is  1 .07):  '); 

if  isempty(confidence) 
confidences  1.07; 
end 

PHIS=confidence;  %( 100-confidence)/ 100;  %[17]  pg.  317 

'Process  noise  applied  is:  \PHIS 

for  i=l:l:statesize; 

Q(i,i)=PHISA2; 

end 

%*****  time  constants  and  noise  variances  for  noise  driving  motion  ***** 

'enter  time  constants  associated  with  motion...' 


tauomegal  =  input(*enter  tau  omega  1  (default=.07)  '); 
if  isempty(tauomegal) 
tauomegal  =.07 
end 

tauomega2  =  input('enter  tau  omega2  (default  =  .07)  '); 
if  isempty(tauomega2) 
tauomega2=.07 
end 

tauomega3  =  input('enter  tau  omega3  (defaults  le7)  '); 
if  isempty(tauomega3) 
tauomega3=le7 
end 

'press  "return"  to  continue...’ 
pause;clc;close; 
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'Enter  the  scaling  parameters  for  the  Unscented  Transform...' 

t  I 

alpha=input('enter  alpha  (default  is  .01):  ’);  ^determines  spread  of  sigma  values  about  mean,  .0001<alpha<l 

if  isempty  (alpha) 
alpha=01 
end 

beta=input('enter  beta  (default  is  2):  ’);  ^incorporates  prior  knowledge  of  state  dist,  beta=2 

for  Gaussian 
if  isempty  (beta) 
beta=2 
end 

augalpha=input('enter  alpha  for  augmented  sigma  (default  is  .01):  '); 
if  isempty  (augalpha) 
augalpha=.01 
end 

^determines  spread  of  sigma  values  about  mean,  .0001<alpha<l 


augbeta-input(*enter  beta  for  augmented  sigma  (default  is  2):  ’); 
if  isempty  (augbeta) 
augbeta=2 
end 

%incorporates  prior  knowledge  of  state  dist,  beta=2  for  Gaussian 


’Please  wait  while  data  is  processed...' 

%*♦***  iQQp  t0  calculate  estimated  states  at  each  time  step  ***** 


count=l; 

t=0.; 


%  initialize  counter 

^initialize  time 


ArrayT(l)=t; 

while  count<=length(omega  l_q_min)- 1 
%*****  Assign  variables  to  more  managable  variable  names  ***** 


x  1  =omega  1  hat(count); 
x2=omega2hat(count); 
x3=omega3hat(count); 
x4=qlhat(count); 


x5=q2hat(count); 

x6=q3hat(count); 

x7=q4hat(count); 


den=sqrt(x4A2+x5A2+x6A2+x7A2); 

den3=denA3; 

f4=(x  l  *x7-x2*x6+x3*x5); 
f5=(xl*x6+x2*x7-x3*x4); 
f6=(-x  1  *x5+x2  *x4+x3  *x7); 
f7=(-x  l  *x4-x2*x5-x3*x6); 


%convenient  compilation  of  terms  to  be  used  later 
^convenient  compilation  of  terms  to  be  used  later 
^convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 


%*****  Current  estimate  of  the  state  vector  ***** 


eststate=[xl  x2  x3  x4  x5  x6  x7]’; 

%*****  Calculate  parameter  values  ***** 


L=statesize; 


%  dimens  ion  of  state  vector 


kappa=3-L; 

lambda=alphaA2*(L+kappa)-L; 

gamma=sqrt(L+lambda); 


%secondaiy  scaling  parameter 
%primary  scaling  parameter 


if  isempty(find(eig(P)<0))  %check  for  positive-definiteness  of  P 

C=chol(P); 
else 

C=zeros(statesize,statesize);  %Cholesky  factorization  of  covariance  matrix 

end 


%*****  Calculate  Sigma  Point  Matrix,  sigma  ***** 


sigma=[  ]; 

sigma(:,l)=eststate;  %Sigma-zero 

for  i=2:l:L+l 

sigma(:,i)=sigma(:1l)+gamma*C(i-l1:)';  %Sigma  points  1  through  L  are  eststate  plus  gamma  times  the 

^transpose  of  the  ith  row  of  the  Cholesky  upper  triangular 
^factorization 
end 

for  i=L+2:l:2*L+l 

sigma(:,i)=sigma(:,l)-gamma*C(i-(Lf  1),:)’;  %Sigma  points  L+l  through  2L  are  eststate  minus  gamma 

%times  the  transpose  of  the  ith  row  of  the  Cholesky  upper 
%triangular  factorization 
end 
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%*****  Instantiate  Sigma  Points  Through  Nonlinear  Function,  sigmabar  ***** 
%*****  Propagate  estimated  states  forward  using  one-step  Euler  integration  ***** 

sigmabar=[  ]; 


forj=l:l:length(sigma(l,:)) 

Xl=sigma(lj); 

X2=sigma(2,j); 

X3=sigma(3,j); 

X4=sigma(4j); 

X5=sigma(5,j); 

X6=sigma(6,j); 

X7=sigma(7,j); 

S=0; 

HI=.01; 

while  S<=(TS-.0001) 

X 1  dot=- 1  /tauomega  1  *X  1 ; 
Xl=Xl+Hl*Xldot; 

X2dot=-  l/tauomega2*X2; 
X2=X2+H  l  *X2dot; 

X3dot=-l/tauomega3*X3; 
X3=X3+H  1  *X3dot; 

X4dot=.5*f4/den; 

X4=X4+Hl*X4dot; 

X5dot=.5*f5/den; 
X5=X5+H  I  *X5dot; 

X6dot=  5*f6/den; 
X6=X6+H  I  *X6dot; 

X7dot=.5*f7/den; 
X7=X7+H1  *X7dot; 


S=S+H1; 

end 


%omegalhat  component  to  be  propagated  forward  one  time  step 
%omega2hat  component  to  be  propagated  forward  one  time  step 
%omega3hat  component  to  be  propagated  forward  one  time  step 
%qlhat  component  to  be  propagated  forward  one  time  step 
%q2hat  component  to  be  propagated  forward  one  time  step 
%q3hat  component  to  be  propagated  forward  one  time  step 
%q4hat  component  to  be  propagated  forward  one  time  step 

^initialize  integration  timer 

%set  step  size  for  numerical  integration 

%Ioops  from  current  step  to  just  short  of  next  time  step 


^increment  integration  timer  by  defined  integration  step  size 
%yields  each  state  estimate  integrated  forward  to  next  time  step 
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sigmabar(  1  j)=X  1 ;  %transformed  sigma  points 

sigmabar(2j)=X2; 
sigmabar(3j)=X3; 
sigmabar(4j)=X4; 
sigmabar(5j)=X5; 
sigmabar(6j)=X6; 
sigmabar(7j)=X7; 

end 

%*****  Compute  the  predicted  mean  of  each  state  ***** 

WmO=lambda/(L+lambda); 

Wm=l/(2*(L+Iambda)); 

Xlbar=WmO*sigmabar(l,l); 

X2bar=WmO*sigmabar(2, 1 ); 

X3bar=WmO*sigmabar(3, 1 ); 

X4bar=WmO*sigmabar(4, 1 );  %this  block  predicts  mean  as  a  weighted  sum 

X5bar=WmO*sigmabar(5, 1 ); 

X6bar=WmO*sigmabar(6, 1 ); 

X7bar=WmO*sigmabar(7, 1); 

fork=2:l:2*L+l 

X 1  bar=X  1  bar+Wm*sigmabar(  1  *k); 

X2bar=X2bar+Wm*sigmabar(2,k); 

X3bar=X3bar+Wm*sigmabar(3,k); 

X4bar=X4bar+Wm*sigmabar(4,k); 

X5bar=X5bar+Wm*sigmabar(5,k); 

X6bar=X6bar+Wm*sigmabar(6,k); 

X7bar=X7bar+Wm*sigmabar(7,k); 

end 

statebar=[X  lbar  X2bar  X3bar  X4bar  X5bar  X6bar  X7bar]';  %  vector  of  projected  states 

%*****  Compute  the  predicted  covariance  before  the  update,  M  ***** 

WcO=Iambda/(L+lambda)+l-alphaA2+beta;  %weight  Wc  for  initial  step 

Wc=l/(2*(L+lambda));  %Wc  for  steps  other  than  zero 

M=WcO*((sigmabar(:,  1  )-statebar)*(sigmabar(:,  1  )-statebar)'); 

for  m=2:l:2*L+l 


%weight  Wm  for  initial  step 
%Wm  for  steps  other  than  zero 
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M=M+Wc*((sigmabar(:,m)-statebar)*(sigmabar(:,m)-statebar)'); 
end 

%*****  Augment  Sigma  point  Matrix  to  Account  for  Process  Noise  Covariance,  Q  ***** 

Lprime=2*L; 
augkappa=3  -Lprime ; 

auglambda=augalphaA2*(Lprime+augkappa)-Lprime; 
auggamma=sqrt(Lprime+auglambda); 
if  isempty(find(eig(Q)<0)) 

C2=chol(Q); 
else 

C2=zeros(statesize,statesize); 
end 

newsigma=sigmabar; 
for  n=l:l:statesize 

newsigma(:,2*L+ 1  +n)=sigmabar(:,  1  )+auggamma*C2(n,:)’; 
end 

for  n=statesize+ 1 : 1 :2*statesize 
newsigma(:,2*L+l^-n)=sigmaba^(:,l)-auggamma*C2(n-L,:),; 
end 

%*****  Instantiate  transformed  Sigma  points  through  non-linear  observation  model  ***** 
%*****  In  this  implementation,  the  observations  are  the  states  (zetabar=newsigma) !***** 

for  p=l :  1  :length(newsigma(  1 ,:)) 
zetabar(  1  ,p)=newsigma(  1  ,p); 
zetabar(2,p)=newsigma(2,p); 
zetabar(3,p)=newsigma(3  ,p) ; 
zetabar(4,p)=newsigma(4,p); 
zetabar(5,p)=newsigma(5,p); 
zetabar(6,p)=newsigma(6,p); 
zetabar(7,p)=newsigma(7,p); 
end 

%*****  Compute  the  predicted  observation  vector,  obsbar  ***** 

augWmO=auglambda/(Lprime+auglambda);  %weight  Wm  for  initial  step 

augWm=  l/(2*(Lprime+auglambda));  %Wm  for  steps  other  than  zero 

zl  bar=augWmO*zetabar(l ,  l ); 
z2bar=augWmO*zetabar(2, 1 ); 


%augment  with  additional  points 
%secondary  scaling  parameter 
%primary  scaling  parameter 
%gamma  for  augmented  matrix 
%check  for  positive-definiteness 

%Cholesky  factorization  of  process  noise  covariance  matrix 
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z3bar=augWmO*zetabar(3, 1 ); 

z4bar=augWmO*zetabar(4,l);  %this  block  predicts  mean  as  a  weighted  sum 

z5ban=augWmO*zetabar(5, 1 ); 
z6bar=augWmO*zetabar(6, 1); 
z7bai^augWmO*zetabar(7, 1); 

for  k=2: 1  :Iength(newsigma(  1 ,:)) 
zlban=zlbar+augWm*zetabar(l  ,k); 
z2bai^z2bar+augWm*zetabar(2,k) ; 
z3bar=z3bar+augWm*zetabar(3  ,k) ; 
z4bar=z4bar+augWm*zetabar(4,k); 
z5bar=z5bar+augWm*zetabar(5,k); 
z6bai^z6bar+augWm*zetabar(6,k); 
z7bar=z7bar+augWm*zetabar(7,k) ; 
end 

obsbar=[zlbar  z2bar  z3bar  z4bar  z5bar  z6bar  z7bar]';  %  vector  of  projected  observations 

%*****  Calculate  Measurement  Noise  Matrix,  RMAT  ***** 

%*****  Uses  A  matrix  from  Gauss_newton  to  relate  6x6  RSENSOR  to  covariance  of  quaternions  after  convergence  [11]  ****** 
RMAT=[ROMEGA,  zeros(3,4);zeros(4,3),  A(:,:,count)*RSENSOR*A(:,:, count)’]; 

%*****  Calculate  Innovation  Covariance  Matrix,  Pzz  ***** 

augWcO=auglambda/(Lprime+auglambda)+ 1  -augalphaA2+augbeta;  %weight  Wc  for  initial  step 
augWc=l/(2*(Lprime+auglambda));  %Wc  for  steps  other  than  zero 

Pzz=augWcO*((zetabar(:,l)-obsbar)*(zetabar(:,  1  )-obsbar)'); 
for  p=2:l  :2*Lprime+l 

Pzz=Pzz+augWc*((zetabar(:,r)-obsbar)*(zetabar(:,r)~obsbar)’); 

end 

Pzz=Pzz+RMAT; 

%*****  Calculate  Cross-Correlation  Matrix,  Pxz  ***** 

Pxz=augWcO*((newsigma(:,  l)-statebar)*(zetabar(:,  l)-obsbar)'); 
for  r=2:l  :2*Lprime+l 

Pxz=Pxz+augWc*((newsigma(:,r)-statebar)*(zetabar(:,r)-obsbar)'); 

end 


%*****  Calculate  Kalman  Gain  Matrix  ***** 


K=Pxz*inv(Pzz); 


%calculate  Kalman  gain  matrix 


%*****  Form  observation  vector  (measurement)  ***** 

obs(  1 :7, count* 1  )=[omega  l_q_min(count+l )  omega2_q_min(count+l)  omega3_q_min(count+ 1 )  q_min(l, count*  1) 
q„min(2,count+ 1 )  q_min(3,  count*  1 )  q_min(4,count+ 1 )] 

%*****  Calculate  updated  state  estimates  using  propagated  states,  residuals  and  Kalman  gains 

statehat=statebar-K*  (obsbar-obs(  1 :7,  count*  1 )); 

%*****  Calculate  Covariance  After  the  Update  ***** 

P=M-K*Pzz*K';  %calcuate  covariance  after  update 

P=sqrt(P.A2); 

%*****  Increment  time  ***** 

t=t+TS;  ^increment  time 

%*****  Check  for  loss  of  data  ***** 


if  q_min(l  :4,count)=[0  0  0  1]'; 

omega  1  hat(count+l  )=omega  1  hat(count); 
omega2hat(count+ 1  )=omega2hat(count); 
omega3hat(count+ 1  )=omega3hat(count); 
q  1  hat(count+ 1  )=q  1  hat(count); 
q2hat(count+ 1  )==q2hat  (count); 
q3hat(count+l  )=q3hat(count); 
q4hat(count+ 1  )=q4hat(count); 
flag(count)=l; 

else 

%***************  management  ***************** 
omega  1  hat(count+ 1  )=statehat(  1 ) ; 
omega2hat(count+ 1  )=statehat(2); 
omega3hat(count+l  )=statehat(3); 
ql  hat(count+ 1  )=statehat(4); 
q2hat(count+ 1  )=statehat(5); 
q3hat(count+l  )=statehat(6); 
q4hat(count+l)=statehat(7); 
end 


ArrayT(count+l)=t; 


%***************  gjj-Qj-  Analysis  ****************** 


SP1  l=sqrt(P(l,l»; 

SP1 1P=-SP1 1; 

SP22=sqrt(P(2,2)); 

SP22P=-SP22; 

SP33=sqrt(P(3,3)); 

SP33P=-SP33; 

SP44=sqrt(P(4,4)); 

SP44P=-SP44; 

SP55=sqrt(P(5,5)); 

SP55P=-SP55; 

SP66=sqrt(P(6,6)); 

SP66P=-SP66; 

SP77=sqrt(P(7,7)); 

SP77P=-SP77; 

ArraySPl  l(count+l)=SPll; 
ArraySPI  1  P(count+ 1  )=SP  1 1 P; 
ArrayS  P22(count+ 1  )=SP22; 
ArraySP22P(count+ 1  )=SP22P; 
ArraySP33(count+l)=SP33; 
ArraySP33P(count+ 1  )=SP33P; 
ArraySP44(count+ 1  )=SP44; 
ArraySP44P(count+ 1  )=SP44P; 
ArraySP55(count+l)=SP55; 
ArraySP55P(count+ 1  )=SP55P; 
ArraySP66(co  u  nt+ 1  )=SP66 ; 
ArraySP66P(count+ 1  )=SP66P; 
ArraySP77(count+l  )=SP77; 
ArraySP77P(count+ 1  )=SP77P; 

count=count+l; 

end 


^Calculate  theoretical  upper  error  bound  on  omega  1  hat 
^Calculate  theoretical  lower  error  bound  on  omega  that 
^Calculate  theoretical  upper  error  bound  on  omega2hat 
^Calculate  theoretical  lower  error  bound  on  omega2hat 
%  Calculate  theoretical  upper  error  bound  on  omega3hat 
^Calculate  theoretical  lower  error  bound  on  omega3hat 
^Calculate  theoretical  upper  error  bound  on  qlhat 
%Calculate  theoretical  lower  error  bound  on  qlhat 
%Calculate  theoretical  upper  error  bound  on  q2hat 
%Calculate  theoretical  lower  error  bound  on  q2hat 
%Calculate  theoretical  upper  error  bound  on  q3hat 
%Calculate  theoretical  lower  error  bound  on  q3hat 
%CaIculate  theoretical  upper  error  bound  on  q4hat 
%  Calculate  theoretical  lower  error  bound  on  q4hat 


%  increment  counter 


%******  calculate  mean  square  errors  for  "instantaneous"  and  for  UKF  ****** 

%******  mean  square  error  excludes  first  5  samples  to  reduce  effects  of  transients  ***** 


s2=length(omega  l_q_min); 
sl=5; 


omega  1  _q_min_mse=mean„square_error(omega  l_q_min(s  1  :s2), omega  1  (s  1  :s2)); 
omega2„q_min_mse=mean_square_error(omega2_q_inin(sl  :s2),omega2(s  1  :s2)); 
omega3„q_min_mse=mean_square_error(omega3_q_inin(sl  :s2),omega3(sl  :s2)); 
q  l_q_min_mse=mean_square„error(q_min(  1  ,(s  1  :s2)),q  1  (s  1  :s2)); 
q2_q_min_mse=mean_square_error(q_min(2,(s  1  :s2)),q2(sl  :s2)); 
q3_q_min_mse=mean_square_error(q_min(3,(s  1  :s2)),q3(s  I  :s2)); 
q4_q_min_mse=mean_square_error(q_min(4,(s  1  :s2)),q4(s  l  ;s2)); 

omega  1  _ukf_mse=mean_square_error(omega  1  hat(s  1  :s2),omega  1  (s  1  :s2)); 
omega2_ukf_mse=mean_square_error(omega2hat(sl  :s2),omega2(sl  :s2)); 
omega3_ukf_mse=mean_square_error(omega3hat(s  1  :s2),omega3(s  1  :s2)); 
q  I  _ukf_mse=mean_square_error(q  I  hat(s  l  :s2),q  1  (s  1  :s2»; 
q2_ukf_mse=mean_square„error(q2hat(s  1  :s2),q2(s  1  :s2»; 
q3„ukf_mse=mean_square„error(q3hat(s  1  :s2),q3(s  1  :s2)); 
q4_ukf„mse=mean_square_error(q4hat(s  t  :s2),q4(s  1  :s2)); 

%***************  plotting  and  output  statements  ***************** 

plots=input('Enter  "1"  to  display  plots.  Enter  ”0”  for  no  plots.  Default  is  no  plots:  '); 
if  isempty(plots) 
plots=0; 
end 

if  plots— 1 


%******  pi0ts  of  error  vs.  true  and  theoretical  error  bounds  ******* 

plot(ArrayT,  (omega  1  ( 1  :s2)-omega  1  hat),  ArrayT,  Arrays  P 1 1  ,'x’,  ArrayT,  ArraySP  1 1  P,'x’); 

grid  on;title('error  in  EKF  estimate  of  omega  1'); 

legend('error  in  omega  1  hat', 'theoretical  bound'); 

xlabel(’time  (sec)');ylabel('eiTor  in  omegalhat  (rad/s)’); 

pause;close;clc; 

plot(ArrayT,(omega2(l:s2)-omega2hat), ArrayT, AiTaySP22,'x',ArrayT,ArraySP22P,'x’); 

grid  on;title(’error  in  EKF  estimate  of  omega2’); 

legend(’error  in  omega2hat', 'theoretical  bound'); 

xlabel('time  (sec)');ylabel('error  in  omega2hat  (rad/s)’); 

pause;close;clc; 

plot(ArrayT,(omega3(l:s2)-omega3hat),  ArrayT,  ArraySP33,’x',ArrayT,ArraySP33P,'x'); 
grid  on;title('error  in  EKF  estimate  of  omega3'); 
legend('error  in  omega3hat*, 'theoretical  bound'); 
xlabel('time  (sec)');ylabel('error  in  omega3hat  (rad/s)'); 
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pause;c!ose;clc; 

plot(ArrayT,(q  1  (1  :s2)-qlhat),  ArrayT,  ArraySP44,'x',ArrayT,  ArraySP44P,’x’); 

grid  on;title('error  in  EKF  estimate  of  ql'); 

legendferror  in  q  that', 'theoretical  bound'); 

xlabelftime  (secy);ylabel('error  in  ql'); 

pause;close;clc; 

plot(ArrayT,(q2(  1  :s2)-q2hat),  ArrayT,  Array  S  P55  ,'x\  ArrayT,  ArrayS  P55  P,'x*) ; 

grid  on;title(’error  in  EKF  estimate  of  q2’); 

legend('error  in  q2hat’, 'theoretical  bound'); 

xlabelftime  (sec)');ylabe!('error  in  q2'); 

pause;close;clc; 

plot(  ArrayT, (q3(l:s2)-q3hat),  ArrayT, ArraySP66,'x',ArrayT,ArraySP66P,'x'); 

grid  on;title('error  in  UKF  estimate  of  q3'); 

legend('error  in  q3  hat', 'theoretical  bound'); 

xlabel('time  (sec)');ylabel('error  in  q3'); 

pause;close;clc; 

plot(ArrayT,(q4(  1  :s2)-q4hat),  ArrayT,  ArraySP77,’x\  ArrayT,  ArraySP77P,’x’); 

grid  on;tit!e('error  in  UKF  estimate  of  q4'); 

legend('error  in  q4hat', 'theoretical  bound’); 

xlabel('time  (sec)');yIabel('error  in  q4'); 

pause;close;clc; 

%******  plots  to  show  true  vs.  quaternion  derived  alone  vs.  UKF  estimates  ******* 

plot(  ArrayT,  omegal_q_min,'gx',ArrayT,  omega  1  hat, 'm',  ArrayT,  omegal  (l:s2),'~');grid  on; 
titleCQuatemion  derived  omega  1,  UKF  estimate  of  omegal,  "True"  omega  1' ); 
xlabelOtime  (sec)’) ;ylabel (’omegal  (rad/s)’); 

legend  (’omegal  from  differencing  quaternions', ’omegal  from  UKF, 'true  omegal'); 
pause;clc;close; 

plot(  ArrayT,  omega2_q_min,'gx',  ArrayT,  omega2hat,'m',  ArrayT,  omega2(l:s2),'--');grid  on; 
titleCQuatemion  derived  omega2,  UKF  estimate  of  omega2,  "True"  omega2' ); 
xlabelftime  (sec)');ylabel('omega2  (rad/s)'); 

legendfomega2  from  differencing  quaternions’, ’omega2  from  UKF,'true  omega2'); 
pause;clc;close; 

plot(  ArrayT,  omega3_q_min,'gx', ArrayT,  omega3hat,'m',  ArrayT,  omega3(l:s2),'~');grid  on; 
titleCQuatemion  derived  omega3,  UKF  estimate  of  omega3,  "True"  omega3’ ); 
xlabelftime  (sec)');ylabel('omega3  (rad/s)’); 

legend(’omega3  from  differencing  quatemions7omega3  from  UKF,'true  omega3'); 
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pause;clc;cIose; 

end 

’Summary  of  performance  of  each  method...' 

The  MSE  in  omega  1  derived  from  differencing  quaternions  is  (rad/s):  7  '.omega l_q_min_mse 

<  l 

'The  MSE  in  omega  1  for  the  UKF  estimates  is  (rad/s):  7  omega  l_ukf„mse 

I  I 

The  MSE  in  omega2  derived  from  differencing  quaternions  is  (rad/s): 7  ’,omega2_q_min_mse 

t « 

The  MSE  in  omega2  for  the  UKF  estimates  is  (rad/s):  7  \omega2_ukf_mse 

'Please  press  "return"  to  continue...' 
pause;clc;close; 

The  MSE  in  omega3  derived  from  differencing  quaternions  is  (rad/s):  7  ',omega3_q_min_mse 

'The  MSE  in  omega3  for  the  UKF  estimates  is  (rad/s):  7  \omega3_ukf_mse 

'The  MSE  in  ql  derived  using  Gauss-Newton  is: ','  \ql_q_min_mse 

'The  MSE  in  ql  for  the  UKF  estimates  is:  7  \ql_ukf_mse 

'Please  press  "return"  to  continue...' 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) ...' 

The  MSE  in  q2  derived  using  Gauss-Newton  is:  7  \q2_q_min_mse 
'The  MSE  in  q2  for  the  UKF  estimates  is:  7  ',q2_ukf_mse 

i  v 

'press  "return"  to  continue...' 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) 

« i 

'The  MSE  in  q3  derived  using  Gauss-Newton  is:  7  \q3_q_min_mse 
'The  MSE  in  q3  for  the  UKF  estimates  is:  7  ',q3_ukf_mse 


The  MSE  in  q4  derived  using  Gauss-Newton  is:  7  \q4_q_min_mse 


'The  MSE  in  q4  for  the  UKF  estimates  is:  7  \q4_ukf_mse 

«  i 

'press  "return"  to  continue...' 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) ...' 

%******  rotate  each  vector  using  actual  and  UKF  attitude  quaternion  ****** 

%******  check  mean-square-error  between  vectors  ****** 

’******  Evaluate  Mean-Square-Error  of  rotated  vectors  using  Gauss-Newton  derived  quaternion  ******' 

'Press  "return"  to  rotate  the  reference  vector  at  each  time  step  to  the' 

'representation  in  the  rotated  frame  using  both  the  actual  rotation  quaternion' 

'and  the  quaternion  derived  using  the  UKF...once  rotation  is  complete,' 

'calculate  the  mean-square-error  between  the  results.' 
pause;close;c!c; 

qukf=[q  1  hat;q2hat;q3hat;q4hat] ;  %  matrix  representation  of  ukf  quaternions 

ukf_mse_erron=[  ]; 
forj=l:count; 

ukfvec(  1 :6,j)=qframerot6(qukf(  1 :4  j),vref(  1 :6  j));  %  rotate  ref  vector  using  min  error  quaternion 

Q_vec(  1 :6,j)=qframerot6(Qvec(l  :4  j),vref(  1 :6,j));  %rotate  ref  vector  using  true  quaternion 

ukf„mse_error(j)-mean_square_error(ukfvec(l:6,j),Q_vec(l:6j));  ^calculate  MSE  between  rotated  vectors 
end 

%*****  determine  angle  between  rotated  vectors  ***** 
for  k=l:count 

ukfsunangle(k)=acos(dot(ukfvec(l  :3,k),Q_vec(  1 :3,k))/(norm(ukfvec(  1 :3,k))*norm(Q_vec(l  :3,k)»); 
ukfmagangle(k)=acos(dot(ukfvec(4:6,k),Q_vec(4:6,k))/(norm(ukfvec(4:6,k))*norm(Q_vec(4:6,k)))); 
GNsunangle(k)=acos(dot(q_minvec(l  :3,k),Q_vec(l  :34c))/(norm(q_minvec(l  :3,k))*norm(Q_vec(l  :3,k)»); 
GNmagangIe(k)=acos(dot(q„minvec(4:6,k),Q_vec(4:6,k))/(norm(q_minvec(4:6,k))*norm(Q_vec(4:6,k)))); 
end 

ukfsunangle^ukfsunangle*  180/pi; 
ukfmagangle=ukfmagangle*  1 80/pi; 

GNsunangle=GNsunangle*  1 80/pi; 

GNmagangle=GNmagangle*  180/pi; 

% ******  plot  vectors  rotated  using  derived  and  actual  quaternions  ***** 

%plot3(Q_vec(  1 , 1  :count),CLvec(2,l  :count),Q_vec(3, 1  :count),’b');grid  on;hoId  on; 

%plot3(ukfvec(  l ,  1  :count),ukfvec(2, 1  :count),ukfvec(3,l  :count),’m'); 

%plot3(Q_vec(4, 1  :count),Q_vec(5, 1  :count),Q_vec(6, 1  :count),’r');grid  on;hold  on; 

%plot3  (ukfvec(4, 1  :count),ukfvec(5, 1  :count),ukfvec(6, 1  :count),*g'); 


%xlabel('X  component');ylabeI('Y  component');zlabel('Z  component'); 

%legend(’sun  vector,  actual  qVsun  vector,  ukf  q'.’mag  field  vector,  actual  q’,  'mag  field  vector,  ukf  q'); 
%title('3-D  plot  of  reference  vectors  rotated  using  actual  quaternions  and  ukf  derived  quaternions'); 
%pause;close;clc; 

%******  plot  mean-square-error  between  reference  vector  rotated  using  actual  quaternion  ****** 
%******  and  reference  vector  rotated  using  derived  quaternion.  ****** 

%plot(T(l  .count), ukf_mse_error(l  :count)), grid  on; 

%title('mean-square  error  between  reference  vectors  rotated  using  ukf  derived  and  actual  quaternions*); 
%xlabel('time  (sec)');ylabel(,MSE');legend('MSE’) 

%'press  "return"  to  continue...’ 

%pause;clo$e;clc; 

******** **************  Overall  Performance  Indicator  ************************* 

*******  Excludes  first  five  measurements  to  reduce  effect  of  transients  ******* 

’The  average  of  the  omega  mean-square-errors  for  the  differencing  method  is:  ’ 

GNomegaperf=(omega  1  _q_min_mse-K>mega2„q__min_mse+omega3„q_min_mse)/3 

The  average  of  the  omega  mean-square-errors  for  the  ukf  results  is:  ’ 
ukfomegaperf=(omegal_ukf_mse+omega2_ukf_mse+omega3_ukf_mse)/3 

'The  average  of  the  quaternion  component  mean-square-errors  for  the  Gauss-Newton  method  is:  ’ 
GNqperf=(q  1  _q_min_mse+q2_q_min_mse+q3_q_min_mse4q4_q_min_mse)/4 

The  average  of  the  quaternion  component  mean-square-errors  for  the  ukf  results  is:  ' 
ukfqperf=(q  1  „ukf_mse+q2„ukf_mse+q3_ukf_mse+q4_ukf_mse)/4 

'press  "return"  to  continue...' 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) ...’ 

'The  average  MSE  for  the  ref  vectors  rotated  using  GN  quaternions:  * 
mean_GN_mse_error=mean(GN_mse_error) 

'The  average  MSE  for  the  ref  vectors  rotated  using  ukf  quaternions:  ’ 
mean_ukf_mse_error=mean(ukf_mse_error) 


'press  "return"  to  continue...' 
pause;clc;close; 


'Summary  of  performance  of  each  method  (cont.) 

'overall  GN  performance:  sqrt[(mean  omega  MSE)A2  +  (mean  rotated  vector  MSE)A2)]' 
overall_GN_perf=sqrt(GNomegaperfA2+mean_GN_mse_errorA2) 

’overall  ukf  performance:  sqrt[(mean  omega  MSE)A2  +  (mean  rotated  vector  MSE)A2)]’ 
overalLukf_perf=sqrt(ukfomegaperfA2+mean_ukf_mse_errorA2) 

%•*************************************************************************************** 
%*** *******************************  ukf summary  *************************  *************** 
^****** ************************************************************************ ********** 
'omega:  \ukfomegaperf 

'quaternion:  \mean_ukf_mse_error 
'overall:  ',overalLukf_perf 
'mean  sun  angle  error:  \mean(ukfsunangle) 

'std  sun  angle  error:  \std(ukfsunangle) 

'max  sun  angle  error:  \max(ukfsunangle) 

'mean  mag  angle  error:  \mean(ukfmagangle) 

'std  mag  angle  error:  ’,std(ukfmagangle) 

'max  mag  angle  error:  \max(ukfmagangle) 

'press  "return"  to  continue...' 
pause;close;clc; 

angles=input('Enter  "1"  to  calculate  attitude  error  angles.  Enter  "0"  for  no  angles.  Default  is  no  angles:  '); 
if  isempty(angles) 
angles=0; 
end 


if  angles— 1 


^Calculate  angles  between  body  frame  rotated  using  estimated  quaternion  and 
%rotated  using  "true"  quaternion 

[ovecl,ovec2,ovec3,oveclhat,ovec2hat,ovec3hat,angl,ang2,ang3]=orientation3(Qvec,  qukf); 
end 


%end  of  program 
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A,  15  Rates_ekf.m 

Rates_ekf.m  is  the  MATLAB  script  that  implements  the  extended  Kalman  filter  (EKF)  designed  in 
Chapter  9.  This  implementation  is  for  the  case  where  rotational  rates  are  directly  measured  by  rate  sensors 
and  these  noisy  measurements  are  available  to  the  filter.  Rates_ekf.m  is  called  by  attitude_filter.m  if  the 
appropriate  option  is  selected  by  the  user. 

%RATES_EKF  -  Discrete  Extended  Kalman  Filter  Implementation 
%Uses  data  generated  by  attitude_truth.m  to  estimate  3-D  body  rates  and 
%the  attitude  quaternion  of  a  sounding  rocket  given  noisy  measurements 
%from  two  attitude  sensors  and  noisy  rate  measuements  from  rate  sensors. 

%Uses  a  Gauss-Newton  optimization  and  an  Extended  Kalman  Filter  (EKF)  to 
%generate  a  best  estimate. 

% 

%Written  by  Mark  Charlton.  Last  modified  28  October  2003. 

%Object  subject  to  rotational  motion  -  non-linear  Equation  of  Motion 
%*****  initialize  parameters  and  matrices  ***** 

clc 


ics=[  ]; 
sigmas=[  ]; 
error=[  ]; 
sensorerror=[  ]; 
Array T=[  ]; 
qlhat=[  ]; 
q2hat=[]; 
q3hat=[  ]; 
q4hat=[  ]; 
omega  lhat=[  ]; 
omega2hat=[  ]; 
omega3hat=[  ]; 
ArraySPll=[]; 
ArraySPl  1P=[  ]; 
ArraySP22=[  ]; 
ArrayS  P22P=[  ]; 
ArraySP33=[  ]; 
ArraySP33P=[  ]; 
ArraySP44=[  ]; 
ArraySP44P=[  ]; 


%initialize  error  multiplier 
^initialize  error  multiplier 
^initialize  Arrays 
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ArraySP55=[  ]; 

ArraySP55P=[  ]; 

Array  SP66=[]; 

Array  SP66P=[  ]; 

ArraySP77=[  ]; 

ArraySP77P=[  ]; 

H 1  =.0 1 ;  %assign  integration  step-size  for  Euler  integration 

states  ize=7;  %system  order 

PHIS=0;  ^process  noise  (measure  of  uncertainty  in  dynamic  model) 

PHI=zeros(statesize,statesize);  ^initialize  state  transition  matrix,  PHI 

Q=zeros(statesize,statesize);  %initialize  process  noise  matrix 

P=zeros(statesize,statesize);  %initialize  covariance  matrix 

HMAT=zeros(statesize,statesize);  ^initialize  linearized  measurement  matrix 

%*****  Assign  initial  estimates  for  omegalhat,  omega2hat,  omega3hat,  qlhat,  q2hat,  q3hat,  q4hat  ***** 

'Do  you  wish  to  enter  initial  estimates  of  the  rotational  body  rates  and  the  quaternion  components' 

'or  let  the  program  use  the  actual  initial  conditions  with  a  user  defined  percent  error  to' 

’make  an  initial  estimate  (default  is  to  let  the  program  do  it)?* 

’If  you  choose  manual  entry,  an  uncertainty  in  the  estimate  is  also  required.  The  default  is  "auto".' 

ics=input('Enter  ”1"  for  manual  entry  and  ”0"  for  automatic:  ’); 
if  isempty(ics) 
ics=0; 
end 

if  ics  —  0 

%*****  base  initialization  on  actual  intial  conditions  with  user  defined  standard  error  ***** 
clc 

error=input('Enter  percentage  error  (+/-)  in  initial  conditions  (i.e.,  -5.2%  =  "-5.2")  Default  is  5%:  '); 
if  isempty(error) 
error=5; 
end 

'percent  error  applied  to  the  initial  conditions  is:  \ error 
error=error/lOO; 

omega  1  hat(l  )=omegal  ( 1  )+omega  l  ( 1  )*error; 

omega2hat(  1  )=omega2(l  )+omega2(  1  )*error; 
omega3hat(  1  )=omega3(  1  )+omega3(  1  )*error; 
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qlhat(l)=ql(l)+ql(l)*error; 
q2hat(  1  )=q2(  1  )+q2(  l )  *error; 
q3hat(  1  )=q3(  l  )+q3(  1  )*error; 
q4hat(  1  )=q4(  1  )+q4(  l  )*error; 

'apriori  estimate  of  omegal  (rad/s):  \omegalhat(l) 
’apriori  estimate  of  omega2  (rad/s):  \omega2hat(l) 
’apriori  estimate  of  omega3  (rad/s):  \omega3hat(l) 
'apriori  estimate  of  ql:  \qlhat(l) 

'apriori  estimate  of  q2:  ',q2hat(l) 

'apriori  estimate  of  q3:  \q3hat(l) 

'apriori  estimate  of  q4:  \q4hat(l) 


%apriori  estimate  of  omegal 
%apriori  estimate  of  omega2 
%apriori  estimate  of  omega3 
%apriori  estimate  of  ql 
%apriori  estimate  of  q2 
%apriori  estimate  of  q3 
%apriori  estimate  of  q4 


%***♦*  Non-zero  entries  of  initial  covariance  matrix  for  auto  initial  cond  ***** 


P(  1 , 1  )=(omega  1  ( 1  )*error+eps)A2 ; 
P(2,2)=(omega2(  1  )*error+eps)A2; 
P(3,3)=(omega3(  1  )*error+eps)A2; 
P(4,4)=(q  1  ( l  )*error+eps)A2; 
P(5,5)=(q2(  l)*error+eps)A2; 
P(6,6)=(q3(l)*error+eps)A2; 
P(7,7)=(q4(  1  )*error+eps)A2; 


%set  at  square  of  uncertainty  in  omegal 
%sct  at  square  of  uncertainty  in  omega2 
%set  at  square  of  uncertainty  in  omega3 
%set  at  square  of  uncertainty  in  ql 
%set  at  square  of  uncertainty  in  q2 
%set  at  square  of  uncertainty  in  q3 
%set  at  square  of  uncertainty  in  q4 


else 


%..,or  can  manually  enter  whatever  intialization  values  you  wish... 


omega lhat(l)=input('PIease  enter  initial  omegal  in  rpm:  '); 
omega2hat(l)=input('Please  enter  initial  omega2  in  rpm:  '); 
omega3hat(l)=input('Please  enter  initial  omega3  in  rpm:  '); 
qlhat(l)=input('Please  enter  initial  ql:  '); 
q2hat(l)=input('Please  enter  initial  q2:  '); 
q3hat(l )=input('Please  enter  initial  q3:  '); 
q4hat(l)=input('Please  enter  initial  q4:  '); 


%apriori  estimate  of  omegal 
%apriori  estimate  of  omega2 
%apriori  estimate  of  omega3 
%apriori  estimate  of  ql 
%apriori  estimate  of  q2 
%  apriori  estimate  of  q3 
%apriori  estimate  of  q4 


omega  1  hat(  1  )=omega  1  hat(  1  )*2*pi/60; 
omega2hat(  1  )=omega2hat(  1  )*2*pi/60; 

omega3hat(l)=omega3hat(l)*2*pi/60;  %convert  manual  omega  entries  to  rad/s 


'For  manual  entry  of  initial  conditions,  estimated  uncertainty  in  the  initial  conditions' 
'is  necessary  to  intialize  the  covariance  matrix...'  ^ 


domegalhat=input('PIease  enter  estimated  uncertainty  in  initial  omegal  (rad/s):  ’); 
domega2hat=input('Please  enter  estimated  uncertainty  in  initial  omega2  (rad/s):  '); 


%  uncertainty  in  initial  estimate  of  omegal 
%uncertainty  in  initial  estimate  of  omega2 
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domega3hat=input('PIease  enter  estimated  uncertainty  in  initial  omega3  (rad/s):  '); 
dqlhat=input(’Please  enter  estimated  uncertainty  in  initial  ql :  *); 
dq2hat=input('Please  enter  estimated  uncertainty  in  initial  q2:  ’); 
dq3hat=input('Please  enter  estimated  uncertainty  in  initial  q3:  *); 
dq4hat=input('Please  enter  estimated  uncertainty  in  initial  q4:  ’); 


%  uncertainty  in  initial  estimate  of  omega3 
%  uncertainty  in  initial  estimate  of  ql 
^uncertainty  in  initial  estimate  of  q2 
%uncertainty  in  initial  estimate  of  q3 
%uncertainty  in  initial  estimate  of  q4 


%***♦*  Non-zero  entries  of  initial  covariance  matrix  for  manual  initial  cond  ***** 


P(  1 , 1  )=(domega  I  hat+eps)A2; 

P(2,2)=(domega2hat+eps)A2; 

P(3,3)=(domega3hat+eps)A2; 

P(4,4)=(dq  1  hat+eps)A2; 

P(5,5)=(dq2hat+eps)A2; 

P(6,6)=(dq3hat+eps)A2; 

p(7,7)=(dq4hat+eps)A2; 

end 


%set  at  square  of  uncertainty  in  initial  estimate  of  omega  1 
%set  at  square  of  uncertainty  in  initial  estimate  of  omega2 
%set  at  square  of  uncertainty  in  initial  estimate  of  omega3 
%set  at  square  of  uncertainty  in  initial  estimate  of  ql 
%set  at  square  of  uncertainty  in  initial  estimate  of  q2 
%set  at  square  of  uncertainty  in  initial  estimate  of  q3 
%set  at  square  of  uncertainty  in  initial  estimate  of  q4 


%insure  initial  quaternion  estimate  is  a  unit  quaternion 

mag=sqrt(q  1  hat(  1  )A2+q2hat(  1  )A24q3hat(  1  )A2+q4hat(  1  )A2); 

q  1  hat(  1  )=q  1  hat(  1  )/mag; 

q2hat(  1  )=q2hat(  1  )/mag; 

q3hat(  l  )=q3hat(  1  )/mag; 

q4hat(  1  )=q4hat(  l  )/mag; 

%*****  Enter  measurement  noise  values  for  use  in  calculating  Measurement  Noise  Matrix,  R  ***** 

'Do  you  wish  to  enter  sensor  measurement  sigmas  manually  or  let  the  program  use  the  actual' 

'sensor  measurement  sigmas  with  a  user  defined  percent  error  to  make  an  initial  estimate’ 

'(default  is  to  let  the  program  do  it)?’ 

sigmas=input('Enter  "1"  for  manual  entry  and  "0"  for  automatic:  '); 
if  isempty(sigmas) 
sigmas=0; 
end 
clc 

if  sigmas  =  0 

<%>*****  base  sensor  noise  matrix  on  actual  sigmas  plus  user  defined  error  ***** 

sensorerror=input('Enter  percentage  error  (+/-)  in  sensor  measurement  sigmas  (i.e.,  -5.2%  =  ”-5.2")  Default  is  0%:  ’); 
if  isempty(sensorerror) 
sensorerror=0; 


254 


end 

'percent  error  applied  to  the  actual  sensor  measurement  sigmas  is:  \sensorerror 
sensorerror=sensorerror/ 1 00; 

SIGOMEGAl=stdomegalmeas;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

SIGOMEGA2=stdomega2meas;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

S  IGOMEG  A3=stdomega3meas;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

estSIGOMEG  A 1  =S  IGOMEG  A 1 +S IGOMEG  A 1  *sensorerror; 
estSIGOMEGA2=SIGOMEGA2+SIGOMEGA2*sensorerror; 
estSIGOMEGA3=SIGOMEGA3+SIGOMEGA3*sensorerror; 
estS  IGS  UNX=S  IGS  UNX+S IGS  UNX  *sensorerror; 
estSIGSUNY=SIGSUNY+SIGSUNY*sensorerror; 
estSIGSUNZ=SIGSUNZ+SIGSUNZ*sensorerror; 
estS  IGM  AGX=S  IG  M  AGX+S IGM  AGX  *sensorerror; 
estSIGMAGY=SIGMAGY+SIGMAGY*$ensorerror; 
estS  IGMAGZ=S  IGM  AGZ+S  IGM  AGZ*sensorerror; 

'apriori  estimate  of  omega l  "measurement"  sigma  (rad/s):  \estSIGOMEGAl 
'apriori  estimate  of  omega2  "measurement"  sigma  (rad/s):  \estS IGOMEG A2 
'apriori  estimate  of  omega3  "measurement"  sigma  (rad/s): estS  IGOMEG  A3 
'apriori  estimate  of  sun  sensor  x  axis  measurement  sigma  (deg):  \estS IGSUNX*  1 80/pi 
'apriori  estimate  of  sun  sensor  y  axis  measurement  sigma  (deg):  \estSIGSUNY*  180/pi 
'apriori  estimate  of  sun  sensor  z  axis  measurement  sigma  (deg):  \estSIGSUNZ*  180/pi 
'apriori  estimate  of  mag  field  sensor  x  axis  measurement  sigma  (deg):  \estSIGMAGX*  180/pi 
'apriori  estimate  of  mag  field  sensor  y  axis  measurement  sigma  (deg):  \estSIGMAGY*  180/pi 
'apriori  estimate  of  mag  field  sensor  z  axis  measurement  sigma  (deg):  ’,estSIGMAGZ*  180/pi 

else 

%...or  can  manually  enter  whatever  intialization  values  you  wish... 

estS  IGOMEG  A  l=input('Please  enter  sigma  for  the  omegal  derived  "measurement"  in  rpm:  ');  %apriori  estimate  of  SIGOMEGA1 
estSIGOMEGA2=input('Please  enter  sigma  for  the  omega2  derived  "measurement"  in  rpm:  ’);  %apriori  estimate  of  SIGOMEGA2 
estS  IGOMEG  A3=input('Please  enter  sigma  for  the  omega3  derived  "measurement"  in  rpm:  ’);  %apriori  estimate  of  SIGOMEGA3 

estSIGOMEG  A 1  =estSIGOMEGA  1  *2*pi/60;  ^convert  estSIGOMEGAl  to  rad/s 

estSIGOMEGA2=estSIGOMEGA2*2*pi/60;  %convert  estS  IGOMEG  A2  to  rad/s 

estSIGOMEG  A3=estSIGOMEGA3*2*pi/60;  ^convert  estSIGOMEG  A3  to  rad/s 


%apriori  estimate  of  SIGOMEGA1 
%apriori  estimate  of  S IGOMEG A2 
%apriori  estimate  of  SIGOMEGA3 
%apriori  estimate  of  S IGSUNX 
%apriori  estimate  of  SIGSUNY 
%apriori  estimate  of  SIGSUNZ 
%apriori  estimate  of  S  IGM  AGX 
%apriori  estimate  of  SIGMAGY 
%apriori  estimate  of  SIGMAGZ 


estSIGSUNX=input('Please  enter  sigma  for  the  sun  sensor  x  axis  measurement  in  deg:  '); 
estSIGSUNY=inputOPIease  enter  sigma  for  the  sun  sensor  y  axis  measurement  in  deg:  '); 
estSIGSUNZ=input('Please  enter  sigma  for  the  sun  sensor  z  axis  measurement  in  deg:  '); 


%apriori  estimate  of  S  IGSUNX 
%apriori  estimate  of  SIGSUNY 
%apriori  estimate  of  SIGSUNZ 
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estSIGM  AGX=input(Tlease  enter  sigma  for  the  mag  field  sensor  x  axis  measurement  in  deg:  ');  %apriori  estimate  of  SIGMAGX 
estSIGM  AG  Y=input(’PIease  enter  sigma  for  the  mag  field  sensor  y  axis  measurement  in  deg:  ’);  %apriori  estimate  of  SIGMAGY 
estSIGMAGZ=input('Please  enter  sigma  for  the  mag  field  sensor  z  axis  measurement  in  deg:  ');  %apriori  estimate  of  SIGMAGZ 


estSIGSUNX=estSIGSUNX*pi/180; 
estS  IGSUN  Y =estSIGS  UN  Y*pi/ 1 80; 
estSIGSUNZ=estS  IGSUNZ*pi/ 1 80; 
estSIGM  AGX=estSIGMAGX*pi/ 1 80; 
estSIGMAGY=estSIGMAGY*pi/180; 
estSIGM  AGZ=estSIGMAGZ*pi/l  80; 

end 

'Please  press  "return"  to  continue...’ 
pause;clc;close; 


%convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 
^convert  from  degrees  to  radians 


%*****  Non-zero  submatrices  of  Measurement  Noise  Matrix,  R  ***** 


ROMEGA=diag([estSIGOMEGAl  A2+eps  estSIGOMEGA2A2+«ps  estSIGOMEGA3A2+eps]>; 

RSENSOR=diag([estSIGSUNXA2+eps  estSIGSUNYA2+eps  estSIGSUNZA2+eps  estSIGM  AGXA2+eps  estSIGM  AG  YA2+eps 
estSIGMAGZA2+eps]); 

%*****  user  input  of  process  noise  to  reflect  uncertainty  in  dynamic  model,  PHIS  ***** 

i  1 

confidence=input('Enter  process  noise  to  reflect  uncertainty  in  model  (default  is  1 12):  ’); 

if  isempty(confidence) 
confidence=l  12; 
end 


PHIS=confidence;  %(1 00-confidence)/ 100;  %[17]pg.  317 

'Process  noise  applied  is:  \PHIS 

’press  "return’’  to  continue...’ 
pause;clc;close; 

%*****  loop  to  calculate  estimated  states  at  each  time  step  ***** 


count=l; 


t=0.; 


^initialize  counter 

%  initialize  time 


ArrayT(l)=t; 


%*****  time  constants  and  noise  variances  for  noise  driving  motion  ***** 
%*****  right  now,  the  time  constants  are  hard-coded  ***** 

'enter  time  constants  associated  with  motion../ 


tauomegal  =  input(*enter  tau  omega  1  (default=36)  '); 
if  isempty(tauomega  1 ) 
tauomega  1=36 
end 

tauomega2  =  input('enter  tau  omega2  (default  =  tauomega  1 )  '); 
if  isempty(tauomega2) 
tauomega2=tauomega  1 
end 

tauomega3  =  input('enter  tau  omega3  (default=le7)  '); 
if  isempty(tauomega3) 
tauomega3=le7 
end 

clc 

while  count<=Iength(omega  1  meas)- 1 


%*****  Assign  variables  to  more  managable  variable  names  ***** 

x  1  =omega  1  hat(count); 
x2=omega2hat(count) ; 
x3=omega3hat(count); 
x4=qlhat(count); 
x5=q2hat  (count); 
x6=q3hat(count); 
x7=q4hat(count); 

den=sqrt(x4A2+x5A2+x6A2+x7A2); 
den3=denA3; 

f4=(xl*x7-x2*x6+x3*x5); 
f5=(xl  *x6+x2*x7-x3*x4); 
f6=(-xl  *x5+x2*x4+x3*x7); 
f7=(-xl  *x4-x2*x5-x3*x6); 


^convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 
^convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 
^convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 


%*****  Calculate  elements  of  linearized  State  Transition  Matrix,  F  ***** 


F(  1 , 1  )=- 1  /tauomega  1 ; 


%dxlxl 


F(l,2)=0; 

F(U)=0; 

F(l,4)=0; 

F(l,5)=0; 

F(l,6)=0; 

F(l,7)=0; 

F(2,l)=0; 

F(2,2)=-  l/tauomega2; 

F(2,3)=0; 

F(2,4)=0; 

F(2,5)=0; 

F(2,6)=0; 

F(2,7)=0; 

F(3,l)=0; 

F(3,2)=0; 

F(3,3)=-l/tauomega3; 

F(3,4)=0; 

F(3,5)=0; 

F(3,6)=0; 

F(3,7)=0; 

F(4,l)=.5*x7/den; 

F(4,2)=.5*(-x6)/den; 

F(4,3)=.5*x5/den; 

F(4,4)=.5*f4*(-x4)/den3; 

F(4,5)=.5*(x3/den+f4*(-x5)/den3); 

F(4,6)=.5*(-x2/den-f4*x6/den3); 

F(4,7)=.5*(xl/den+f4*(-x7)/den3); 

F(5,l)=.5*x6/den; 

F(5,2)=,5*x7/den; 

F(5,3)=.5*(-x4)/den; 

F(5,4)=  5*(-x3/den+f5*(-x4)/den3); 

F(5,5  )=  5  *  f5  *(-x5)/den3 ; 

F(5,6)=.5*(xl/den+f5*(-x6)/den3); 

F(5t7)=.5*(x2/den+f5*(-x7)/den3); 

F(6, 1  )=.5*(-x5)/den; 

F(6>2)=.5*x4/den; 

F(6,3)=-5*x7/den; 

F(6,4)=.5*(x2/den+f6*(-x4)/den3); 

F(6,5)=.5*(-xl/den+f6*(-x5)/den3); 

F(6,6)=.5*f6*(-x6)/den3; 

F(6,7)=.5*(x3/den+f6*(-x7)/den3); 

F(7, 1  )=.5*(-x4)/den; 

F(7,2)=.5*(-x5)/den; 

F(7,3)=-5*(-x6)/den; 


%dxlx2 

%dxlx3 

%dxlx4 

%dxlx5 

%dxlx6 

%dxlx7 

%dx2xl 

%dx2x2 

%dx2x3 

%dx2x4 

%dx2x5 

%dx2x6 

%dx2x7 

%dx3xl 

%dx3x2 

%dx3x3 

%dx3x4 

%dx3x5 

%dx3x6 

%dx3x7 

%dx4xl 

%dx4x2 

%dx4x3 

%dx4x4 

%dx4x5 

%dx4x6 

%dx4x7 

%dx5xl 

%dx5x2 

%dx5x3 

%dx5x4 

%dx5x5 

%dx5x6 

%dx5x7 

%dx6xl 

%dx6x2 

%dx6x3 

%dx6x4 

%dx6x5 

%dx6x6 

%dx6x7 

%dx7xl 

%dx7x2 

%dx7x3 
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F(7,4)=.5*(-xl/den+f7*(-x4)/den3);  %dx7x4 

F(7,5)=.5*(-x2/den+f7*(-x5)/den3);  %dx7x5 

F(7,6)=.5*(-x3/den+f7*(-x6)/den3);  %dx7x6 

F(7,7)=.5*f7*(-x7)/den3;  %dx7x7 

%•****  Use  first  two  terms  of  infinite  Taylor  Series  expansion  to  form  non-zero  elements  of  Discrete  Fundamental  Matrix,  PHI 
***** 


PHI=eye(7)+F*t; 


%*****  Calculate  non-zero  elements  of  Discrete  Process  Noise  Covariance  Matrix  (old  Q)***»* 


numl=(.5*TSA2-TSA3/(3*tauomegal));  ^convenient  compilation  of  terms  to  be  used  later 

num2=(.5*TSA2-TSA3/(3*tauomega2»;  %convenient  compilation  of  terms  to  be  used  later 

num3=(.5*TSA2-TSA3/(3*tauomega3));  %convenient  compilation  of  terms  to  be  used  later 

Q(l,l)=estSIGOMEGAlA2*(TS-TSA2/tauomegal+TSA3/(3*tauomegalA2)); 

Q(l,4)=estSIGOMEGAlA2*F(4,l)*numl; 

Q(l,5)=estSIGOMEGAlA2*F(l,5)*numl; 

Q(l,6)=estSIGOMEGAlA2*F(l,6)*numl; 

Q(  1 ,7)=estSIGOMEG  A 1 A2*  F(  1 ,7)*numl ; 

Q(2,2)=estSIGOMEGA2A2*(TS-TSA2/tauomega2*TSA3/(3*tauomega2A2)); 

Q(2,4)=estSIGOMEGA2A2*F(4,2)*num2; 

Q(2,5)=estSIGOMEGA2A2*F(5,2)*num2; 

Q(2,6)=estSIGOMEGA2A2*F(6,2)*num2; 

Q(2,7)=estSIGOMEGA2A2*F(7,2)*num2; 

Q(3,3)=estSIGOMEGA3A2*(TS-TSA2/tauomega3*TSA3/(3*tauomega3A2)); 

Q(3,4)=estSIGOMEGA3A2*F(4,3)»num3; 

Q(3,5)=estSIGOMEGA3A2*F(5,3)*num3; 

Q(3,6)=estSIGOMEGA3A2*F(6,3)*num3; 

Q(3,7)=estSIGOMEGA3A2*F(7,3)*num3; 

Q(4,1)=Q(1,4); 

Q(4,2)=Q(2,4); 

Q(4,3)=Q(3,4); 

Q(4,4)=estSIGOMEGAlA2*(F(4,l)A2+estSIGOMEGA2A2»F(4,2)A2+estSIGOMEGA3A2»F(4,3)A2)*TSA3/3; 

Q(5,1)=Q(1,5); 

Q(5,2)=Q(2,5); 

Q(5,3)=Q(3,5); 

Q(5,4)=Q(4,5); 

Q(5,5)=estSIGOMEGAlA2*(F(5,l)A2+estSIGOMEGA2A2*F(5,2)A2+estSIGOMEGA3A2*F(5,3)A2)*TSA3/3; 

Q(5l6)=(estSIGOMEGAlA2*F(5,l)*F(6,l)+estSIGOMEGA2A2*F(5,2)*F(6,2)+estSIGOMEGA3A2*F(5,3)*F(6,3))*TSA3/3; 

Q(5,7)=(estSIGOMEGAIA2*F(5,l)*F(7,l)+estSIGOMEGA2A2*F(5,2)*F(7,2)+estSlGOMEGA3A2*F(5,3)*F(7,3))*TSA3/3; 

Q(6,1)=Q(1,6); 

Q(6,2)=Q(2,6); 


Q(6,3)=Q(3,6); 

Q(6,4)=Q(4,6); 

Q(6t5)=Q(5,6); 

Q(6,6)=estSIGOMEGAlA2*(F(6,l)A2+estSIGOMEGA2A2*F(6,2)A2+estSIGOMEGA3A2*F(6,3)A2)*TSA3/3; 

Q(6,7)=(estSIGOMEGAlA2*F(6,l)*F(7,I)+estSIGOMEGA2A2*F(6,2)*F(7,2)+estSIGOMEGA3A2*F(6,3)*F(7,3))*TSA3/3; 

Q(7,1)=Q(1,7); 

Q(7,2)=Q(2,7); 

Q(7,3)=Q(3,7); 

Q(7,4)-Q(4,7); 

Q(7,5)=Q(5,7); 

Q(7,6)=Q(6,7); 

Q(7,7)=estSIGOMEGAlA2*(F(7J)A2+estSIGOMEGA2A2!i!F(7,2)A2+estSIGOMEGA3A2*F(7,3)A2)*TSA3/3; 

Q=PHIS*Q; 

%*****  Calculate  covariance  before  update,  M  ***** 

PHrr=PHI';  Recalculate  transpose  of  PHI; 

PHIP=PHI*P; 

PHIPPHIT=PHIP*PHIT; 

M=PHIPPHIT+Q;  Recalculate  covariance  before  update 

%(using  old  P,  old  PHI,  and  old  Q) 

t=t+TS;  %increment  time 

%*****  Propagate  estimated  states  forward  using  one-step  Euler  integration  ***** 

S=0;  Reinitialize  integration  timer 

while  S<=(TS-.0001 )  Reloops  from  current  step  to  just  short  of  next  time  step 

x  1  dot=-  1/tauomega  1  *x  1 ; 
xl=xl+Hl*xldot; 

x2dot=-  l/tauomega2  *x2; 
x2=x2+Hl*x2dot; 

x3dot=-  l/tauomega3*x3; 
x3=x3+Hl*x3dot; 

x4dot=.5*f4/den; 

x4=x4+HI*x4dot; 
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x5dot=.5*f5/den; 

x5=x5+Hl*x5dot; 

x6dot=.5*f6/den; 

x6=x6+Hl*x6dot; 

x7dot=  5*f7/den; 
x7=x7+Hl*x7dot; 

S=S+H  1 ;  ^increment  integration  timer  by  defined  integration  step  size 

end  %yields  each  state  estimate  integrated  forward  to  next  time  step 

%*****  assign  values  from  numerical  integration  to  arrays  ***** 

xbar=[x  l  ;x2;x3;x4;x5;x6;x7] ; 

omega  1  bar(count+ 1  )=x  1 ; 
omega2bar(count+ 1  )=x2; 
omega3bar(count+ 1  )=x3 ; 
ql  bar(count+ 1  )=x4; 
q2bar(count+ 1  )=x5 ; 
q3bar(count+l)=x6; 
q4bar(count+ 1  )=x7 ; 

%*****  Calculate  non-zero  elements  of  the  linearized  measurement  matrix  ***** 

HMAT=eye(7);  %in  this  implementation,  measurements  are  the  states 

%*****  Measurement  Noise  Matrix,  RMAT  ***** 

%*****  Uses  A  matrix  from  Gauss_newton  to  relate  6x6  RSENSOR  to  covariance  of  quaternions  after  convergence  [11]  ****** 
RMAT=[ROMEGA,  zeros(3,4);zeros(4,3),  A(:,:,count)*RSENSOR*A(:,:,count)’3; 

% *****  Calculate  Kalman  gain  matrix  ***** 

HT=HM  AT’ ;  ^calculate  transpose  of  H 1  (next  time  step) 


HM=HMAT*M; 
HMHT=HM*HT; 
HMHTR=HMHT+RM  AT ; 
HMHTRINV=inv(HMHTR); 
MHT=M*HT; 


GAIN=MHT*HMHTRINV; 


^Calculate  Kalman  gain  matrix 


261 


KH=GAIN*HMAT; 

IKH=eye(statesize)-KH; 

%*****  Calculate  covariance  matrix  after  update,  P  ***** 


P=IKH*M;  %CaIculate  covariance  after  update  (new  P) 

%*****  Calculate  residuals:  actual  new  measurement  -  projected  new  measurement  ***** 


%*****  Since  the  H  matrix  is  the  identity  matrix  in  this  implementation,  the  "zbar",  ***** 
%*****  or  projected  measurement,  is  simply  the  projected  state  (zbar=xbar)!  ***** 


omega  1  res=(omega  1  meas(count+  l)-xl); 
omega2res=(omega2meas  (count+ 1  )-x2); 
omega3  res=(omega3meas(count+ 1  )-x3); 
ql  res=q_min(  1  ,count+  l)-x4; 
q2res=q_min(2, count*  1  )-x5 ; 
q3res=q_min(3,count+l  )-x6; 
q4res=q„min(4, count* 1  )-x7 ; 


%calculate  residual  from  propagated  state  and  measurement 
^calculate  residual  from  propagated  state  and  measurement 
Recalculate  residual  from  propagated  state  and  measurement 
%calcu!ate  residual  from  propagated  state  and  measurement 
%calculate  residual  from  propagated  state  and  measurement 
^calculate  residual  from  propagated  state  and  measurement 
%calculate  residual  from  propagated  state  and  measurement 


residual=[omega  1  res;omega2res ;omega3res;q  1  res;q2res;q3res;q4res] ;  %form  vector  of  residuals 


%*****  Calculate  updated  state  estimates  using  propagated  states  and  Kalman  gains 


xhat=xbar+GAIN*residuaI; 


%*****  Check  for  loss  of  data  ***** 


if  q_min(l:4,count)=[0  0  0  1]’; 
omega  1  hat(count+ 1  )=omega  1  hat(count) ; 
omega2hat(count+ 1  )=omega2hat(count) ; 
omega3hat(count+l)=omega3hat(count); 
qlhat(count+l)=qlhat(count); 
q2hat(count+ 1  )=q2hat(count); 
q3hat(count+l)=q3hat(count); 
q4hat(count+l  )=q4hat(count) ; 
flag(count)=l; 

else 

%***************  £>ata  management  ***************** 


omegal  hat(count+l  )=xhat(l ); 
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omega2hat(count+ 1  )=xhat(2) ; 
omega3hat(count+l)=xhat(3); 
ql  hat(count+ 1  )=xhat(4); 
q2hat(count+ 1  )=xhat(5); 
q3hat(count+ 1  )=xhat(6); 
q4hat(count+  l)=xhat(7); 
flag(count)=0; 
end 

ArrayT(count+l  )=t; 

<^***************  £rror  Analysis  ****************** 

SPll=sqrt(P(l,l)); 

SP11P=-SP11; 

S  P22=sqrt(P(2,2)) ; 

SP22P=-SP22; 

SP33=sqrt(P(3,3)); 

SP33P=-SP33; 

SP44=sqrt(P(4,4)); 

SP44P=-SP44; 

SP55=sqrt(P(5,5)); 

SP55P=-SP55; 

SP66=sqrt(P(6,6)); 

SP66P=-SP66; 

SP77=sqrt(P(7,7)); 

SP77P=-SP77; 

ArrayS  PI  l(count+l)=SPl  1; 

ArraySPl  lP(count+l)=SPllP; 
ArraySP22(count+l)=SP22; 

ArrayS  P22P(count+ 1  )=SP22P; 

ArraySP33(count+l  )=SP33; 
ArraySP33P(count+l)=SP33P; 

ArrayS  P44(cou  nt+ 1  )=S  P44 ; 

ArrayS  P44P(count+ 1  )=SP44P; 

ArrayS  P55(count+l  )=SP55 ; 
ArraySP55P(count+l)=SP55P; 

ArrayS  P66(count+l  )=SP66; 

ArrayS  P66P(count+ 1  )=SP66P; 

ArraySP77(count+l  )=SP77; 

ArraySP77P(count+ 1  )=SP77P; 


^Calculate  theoretical  upper  error  bound  on  omega lhat 
%Calculate  theoretical  lower  error  bound  on  omegalhat 
%Calculate  theoretical  upper  error  bound  on  omega2hat 
%Calculate  theoretical  lower  error  bound  on  omega2hat 
%Calculate  theoretical  upper  error  bound  on  omega3hat 
%Calculate  theoretical  lower  error  bound  on  omega3hat 
%Calculate  theoretical  upper  error  bound  on  q lhat 
%Calculate  theoretical  lower  error  bound  on  qlhat 
^Calculate  theoretical  upper  error  bound  on  q2hat 
%Calculate  theoretical  lower  error  bound  on  q2hat 
%Calculate  theoretical  upper  error  bound  on  q3hat 
^Calculate  theoretical  lower  error  bound  on  q3hat 
^Calculate  theoretical  upper  error  bound  on  q4hat 
%Calculate  theoretical  lower  error  bound  on  q4hat 


count=count+l; 


%increment  counter 
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end 

%******  calculate  mean  square  errors  for  "instantaneous"  and  for  EKF  ****** 

%******  mean  square  error  excludes  first  5  samples  to  reduce  effects  of  transients  ***** 

s2=length(omega  1  meas); 
sl=5; 


omega  l  meas_mse=mean_square_error(omegal  meas(s  1 :  s2),  omega  1  (si  :s2»; 
omega2meas_mse=mean_square_error(omega2meas(sl  :s2),omega2(sl  :s2)); 
omega3meas_mse=mean_square_error(omega3meas(sl  :s2),omega3(s  1  :s2)); 
q  l_q_min_mse=mean_square_error(q„min(  1  ,(s  1  :s2)),q  1  (s  1  :s2)); 
q2_q_min__mse=mean_square_error(q_min(2,(s  1  :s2)),q2(s  1  :s2)); 
q3_q_min_mse=mean_square_error(q_min(3,(s  1  :s2)),q3(s  1  :s2)); 
q4_q_min_mse=mean_square_error(q_min(4,(s  1  :s2)),q4(s  1  :s2)); 

omegal_ekfLmse=mean_square_error(omegalhat(sl  :s2),omegal(sl  :s2)); 
omega2_ekf_mse=mean_square„error(omega2hat(sl  :s2),omega2(sl  :s2)); 
omega3_ekflmse=mean_square_error(omega3hat(s  1  :s2),omega3(sl  :s2)); 
ql_ekf_mse=mean„square_error(qlhat(s  1  :s2),ql  (si  :s2»; 
q2_ekf_mse=mean_square_error(q2hat(s  1  :s2),q2(s  1  :s2»; 
q3_ekf„mse=mean„square_error(q3hat(s  1  :s2),q3(s  1  :s2»; 
q4_ekf_mse=mean_square_error(q4hat(s  1  :s2),q4(sl  :s2)); 

%***************  Plotting  and  output  statements  ***************** 

plots=input('Enter  "1"  to  display  plots.  Enter  "0"  for  no  plots.  Default  is  no  plots:  '); 
if  isempty(plots) 
plots=0; 
end 

if  plots— 1 

%******  plots  of  error  vs.  true  and  theoretical  error  bounds  ******* 

plot(ArrayT, (omega  1(1  :s2)-omegalhat),ArrayT,AiTaySPl  l,'x\ArrayT,ArraySPl  lP,'x'); 

grid  on;title('error  in  EKF  estimate  of  omega  1'); 

legend(’error  in  omega  lhatVtheoretical  bound'); 

xlabel('time  (sec)');yIabelCerror  in  omega  1  hat  (rad/s)'); 

pause;close;c!c; 

plot(ArrayT,(omega2(l:s2)-omega2hat),ArrayT,ArraySP22,,x,,ArrayT,ArraySP22P,,x’); 
grid  on;title('error  in  EKF  estimate  of  omega2’>; 
legend('error  in  omega2hat\theoretical  bound'); 


xlabel('time  (sec)');ylabel(’error  in  omega2hat  (rad/s)'); 
pause;close;clc; 

plot(  ArrayT, (omega3  ( 1  :s2)-omega3  hat),  ArrayT,  Array  S  P3  3  ,'x\  ArrayT,  ArraySP3  3  P,'x’) ; 

grid  on;titleCerror  in  EKF  estimate  of  omega3'); 

legend('error  in  omega3  hat', 'theoretical  bound'); 

xlabel('time  (sec)’);ylabel('error  in  omega3hat  (rad/s)'); 

pause;close;clc; 

plot(  ArrayT,(q  1  ( 1  :s2)-ql  hat),  ArrayT,  ArraySP44,’x',  ArrayT,  ArrayS  P44P,'x'); 

grid  on;title('error  in  EKF  estimate  of  ql'); 

legend(*error  in  qlhat’/theoretical  bound'); 

xlabel('time  (sec)');ylabel('error  in  ql'); 

pause;close;clc; 

plot(  ArrayT, (q2(l:s2)-q2hat),  ArrayT,  ArraySP55,,x,,ArrayT,ArraySP55P,'x’); 

grid  on;title(’error  in  EKF  estimate  of  q2'); 

legend('error  in  q2hat','theoretical  bound'); 

xlabel('time  (sec),);ylabel(1error  in  q2'); 

pause;close;clc; 

plot(ArrayT,(q3(l:s2)-q3hat),ArrayT,AiTaySP66,’x', ArrayT, ArraySP66P,'x'); 

grid  on;title('error  in  EKF  estimate  of  q3'); 

legend('error  in  q3hat', 'theoretical  bound'); 

xlabel('time  (sec)');ylabel('error  in  q3'); 

pause;close;clc; 

plot(  ArrayT, (q4(  1  :s2)-q4hat),  ArrayT,  ArraySP77,’x',  ArrayT,  ArraySP77P,'x'); 

grid  on;title('error  in  EKF  estimate  of  q4'); 

legend('error  in  q4hat', 'theoretical  bound'); 

xlabel('time  (sec)');y!abel('error  in  q4’); 

pause;close;c!c; 

%******  plots  to  show  true  vs.  quaternion  derived  alone  vs.  EKF  estimates  ******* 

plot(  ArrayT,  omega lmeas,'gx', ArrayT,  omega  1  hat, 'm',  ArrayT,  omegal(l:s2),'--*);grid  on; 

tit!e('omegal  from  sensor,  EKF  estimate  of  omega  l,  "True"  omega  V  ); 

xlabel('time  (sec)');yIabel('omegal  (rad/s)'); 

legend('omegal  from  rate  sensor','omegal  from  EKF, 'true  omega  1'); 

pause;clc;close; 

plot(  ArrayT,  omega2meas,'gx*, ArrayT,  omega2hat,'m\  ArrayT,  omega2(  1  :s2),’-’,'MarkerSize',5);grid  on; 
titIe('omega2  from  sensor,  EKF  estimate  of  omega2,  "True"  omega2' ); 
xlabel('time  (sec)’);yIabel('omega2  (rad/s)'); 


legend('omega2  from  rate  sensor1, 'omega2  from  EKF, 'true  omega2’); 
pause;clc;cIose; 

plot(  ArrayT,  omega3meas,'gx\ArrayT,  omega3hat,'m\  ArrayT,  omega3(l:s2),’--7MarkerSize’,5);grid 

title(’omega3  from  sensor,  EKF  estimate  of  omega3,  "True*'  omega3' ); 

xlabel('time  (sec),);ylabel(,omega3  (rad/s)’); 

legend(’omega3  from  rate  sensorVomega3  from  EKF, ’true  omega3’); 

pause;clc;c!ose; 

end 

’Summary  of  performance  of  each  method...’ 

The  MSE  in  omegal  from  the  rate  sensor  is  (rad/s):  7  \omegalmeas_mse 
The  MSE  in  omegal  for  the  EKF  estimates  is  (rad/s):  7  \omegal__ekf_mse 
’The  MSE  in  omega2  from  the  rate  sensor  is  (rad/s):  7  \omega2meas_mse 
The  MSE  in  omega2  for  the  EKF  estimates  is  (rad/s):  7  \omega2_ekf_mse 

♦  i 

’Please  press  "return”  to  continue...’ 
pause;clc;c!ose; 

’The  MSE  in  omega3  derived  from  the  rate  sensor  is  is  (rad/s):  7  \omega3meas_mse 

The  MSE  in  omega3  for  the  EKF  estimates  is  (rad/s):  7 1,omega3_ekf_mse 

’The  MSE  in  ql  derived  using  Gauss-Newton  is:  7  \ql_q_min_mse 

’The  MSE  in  ql  for  the  EKF  estimates  is:  7  \ql_ekf_mse 

’Please  press  "return"  to  continue...’ 
pause;clc;close; 

’Summary  of  performance  of  each  method  (cont.) .7 

The  MSE  in  q2  derived  using  Gauss-Newton  is:  ’,’  \q2_q_min_mse 

The  MSE  in  q2  for  the  EKF  estimates  is:  7  \q2_ekf_mse 

’press  ’’return"  to  continue...’ 
pause  ;clc;close; 


’Summary  of  performance  of  each  method  (cont.) ...’ 


'The  MSE  in  q3  derived  using  Gauss-Newton  is: \q3_q_min_mse 


'The  MSE  in  q3  for  the  EKF  estimates  is:  ',q3_ekf_mse 

The  MSE  in  q4  derived  using  Gauss-Newton  is:  7  \q4_q_min_mse 

The  MSE  in  q4  for  the  EKF  estimates  is:  7  *,q4_ekf_mse 

'press  "return"  to  continue...' 
pause;cIc;c!ose; 

'Summary  of  performance  of  each  method  (cont.) ...' 

*******  Evaluate  Mean-Square-Error  of  rotated  vectors  using  Gauss-Newton  derived  quaternion  ******* 

'Press  "return"  to  rotate  the  reference  vector  at  each  time  step  to  the' 

'representation  in  the  rotated  frame  using  both  the  actual  rotation  quaternion' 

'and  the  quaternion  derived  using  the  ekf...once  rotation  is  complete,’ 

'calculate  the  mean-square-error  between  the  results.’ 
pause;close;clc; 

qekf=[ql  hat;q2hat;q3hat;q4hat];  %  matrix  representation  of  ekf  quaternions 

ekf_mse_erroi^[  ]; 
for  j=l:count; 

ekfvec(l  :6,j)=qframerot6(qekf(l  :4  j),vref(l  :6  j));  %rotate  ref  vector  using  min  error  quaternion 

Q_vec(  1 :6j)=qframerot6(Qvec(  1 :4  j),vref(  1 :6,j));  %rotate  ref  vector  using  true  quaternion 

ekf_mse_error(j)=mean_square_error(ekfvec(l:6j),Q_vec(l:6j));  ^calculate  MSE  between  rotated  vectors 
end 

for  k=l  :count 

oekfsunangle(k)=acos(dot(ekfvec(l  :3,k),Q_vec(l  :34c))/(norm(ekfvec(  1 :3,k))*norm(Q_vec(l  :3,k)))); 
oekfmagangle(k)=acos(dot(ekfvec(4:6,k),Q_vec(4:6,k))/(norm(ekfvec(4:6,k))*norm(Q_vec(4:6,k)))); 
oGNsunangle(k)=acos(dot(q_minvec(  1 :3,k),Q_vec(  1 :3,k))/(norm(q_minvec(  1 :3,k))*norm(Q_vec(  1 :3,k)»); 
oGNmagangle(k)=acos(dot(q_minvec(4:6,k)1Q_vec(4:6,k))/(norm(q_minvec(4:6,k))*norm(Q_vec(4:6,k)))); 
end 

oekfsunangle=oekfsunangle*  1 80/pi ; 
oekfmagangle=oekfmagangle*  1 80/pi; 
oGNsunangle=oGNsunangle*  180/pi; 
oGNmagangle^oGNmagangle*  1 80/pi; 

%******  Plot  vectors  rotated  using  derived  and  actual  quaternions  ***** 

%plot3(Q_vec(l,l:count),Q_vec(2,l  :count),Q_vec(3,l  :count),'b');grid  on;ho!d  on; 

%plot3(ekfvec(l ,  1  :count),ekfvec(2, 1  :count),ekfvec(3,l  :count),'m'); 


%plot3(Q_vec(4,l  :count),Q_vec(5,l:count),Q_vec(6,l:countyr');grid  on;hold  on; 

%p!ot3(ekfvec(4, 1  :count),ekfvec(5, 1  :count),ekfvec(6, 1  :count),’g’); 

%xlabel(’X  component');ylabel('Y  component');zlabel('Z  component'); 

%Iegend('sun  vector,  actual  q\'sun  vector,  ekf  q','mag  field  vector,  actual  q\  'mag  field  vector,  ekf  q’); 
%title('3-D  plot  of  reference  vectors  rotated  using  actual  quaternions  and  ekf  derived  quaternions'); 
%pause;close;cIc; 

%******  Plot  mean-square-error  between  reference  vector  rotated  using  actual  quaternion  ****** 
%******  and  reference  vector  rotated  using  derived  quaternion.  ****** 

%plot(T(l  :count),ekfjnse_error(l  :count)),grid  on; 

%titleCmean-square  error  between  reference  vectors  rotated  using  ekf  derived  and  actual  quaternions'); 
%xlabel(,time(sec)*);ylabel(,MSE');legend('MSE') 

%’press  "return”  to  continue...' 

%pause;close;clc; 

**********************  Overall  Performance  Indicator  ************************* 

*******  Excludes  first  five  measurements  to  reduce  effect  of  transients  ******* 

'The  average  of  the  omega  mean-square-errors  from  the  rate  sensors  is:  ’ 

GNomegaperf=(omega  1  meas_mse+omega2meas_mse+omega3meas_mse)/3 

'The  average  of  the  omega  mean-square-errors  for  the  ekf  results  is:  ' 
ekfomegaperf=(omega  1  „ekf_mse+omega2_ekf_mse+omega3_ekf_mse)/3 

'The  average  of  the  quaternion  component  mean-square-errors  for  the  Gauss-Newton  method  is:  ' 
GNqperf=(ql_q_min_mse+q2_q_min„mse+q3_q_min_mse+q4_q_min_mse)/4 

’The  average  of  the  quaternion  component  mean-square-errors  for  the  ekf  results  is:  ’ 
ekfqperf=(ql_ekf_mse+q2_ekf_mse+q3_ekLmse+q4_ekf_mse)/4 

'press  "return"  to  continue...' 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) ...' 

'The  average  MSE  for  the  ref  vectors  rotated  using  GN  quaternions:  ' 
mean_GN_mse_error=mean(GN_mse_error) 

'The  average  MSE  for  the  ref  vectors  rotated  using  ekf  quaternions:  ’ 
mean_ekf_mse_error=mean(ekf_mse_error) 


'press  "return"  to  continue...’ 
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pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) 

’overall  GN  performance:  sqrt[(mean  omega  MSE)A2  +  (mean  rotated  vector  MSE)A2)]’ 
overall_GN_perf=sqrt(GNomegaperfA2+mean_GN_mse_errorA2) 

'overall  ekf  performance:  sqrt[(mean  omega  MSE)A2  +  (mean  rotated  vector  MSE)A2)]’ 
overalLekf_perf=sqrt(ekfomegaperfA2+mean_ekf_mse_errorA2) 

%*******  ***********************************************************************  *********** 
<^<«******* ****** *******************  ekf summary  **************************************** 
%•**********************************************♦****************************** *********** 
’omega:  \ekfomegaperf 

'quaternion:  ',mean_ekf_mse_error 
'overall:  \overall_ekf_perf 
'mean  sun  angle  error:  \mean(oekfsunangIe) 

'std  sun  angle  error:  \std(oekfsunangle) 

'max  sun  angle  error:  \max(oekfsunangle) 

'mean  mag  angle  error:  ’,mean(oekfmagangle) 

'std  mag  angle  error:  \std(oekfmagangIe) 

'max  mag  angle  error:  \max(oekfmagangle) 

'press  "return"  to  continue...' 
pause;c!ose;clc; 

angles=input('Enter  "1"  to  calculate  attitude  error  angles.  Enter  ”0"  for  no  angles.  Default  is  no  angles:  '); 
if  isempty(angles) 
angles=0; 
end 


if  angles==  l 


%Calculate  angles  between  body  frame  rotated  using  estimated  quaternion  and 
^rotated  using  "true"  quaternion 

[ovec  1  ,ovec2,ovec3,ovec  1  hat,ovec2hat,ovec3hat,angl  ,ang2,ang3]=orientation3(Qvec,  qekf); 

end 


%end  of  program 
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A.  16  Rates_ukf.m 

Rates_ukf.  m  is  the  MATLAB  script  that  implements  the  unscented  Kalman  filter  (UKF)  designed  in 
Chapter  9.  This  implementation  is  for  the  case  where  rotational  rates  are  directly  measured  by  rate  sensors 
and  the  noisy  measurements  are  made  available  to  the  filter.  Rates_ukf. m  is  called  by  attitude_fiiter.m  if 
the  appropriate  option  is  selected  by  the  user. 

%RATES_UKF  -  Discrete  Unscented  Kalman  Filter  Implementation 
%Uses  data  generated  by  attitude_sim.m  to  estimate  3-D  body  rates  and 
%the  attitude  quaternion  of  a  sounding  rocket  given  noisy  measurements 
%from  two  attitude  sensors  and  from  rate  sensors.  Uses  a  Gauss-Newton 
%optimization  and  an  Unscented  Kalman  Filter  (UKF)  to  generate  a  best  estimate. 

% 

%Written  by  Mark  Charlton.  Last  modified  9  November  2003. 

%Object  subject  to  rotational  motion  —  non-linear  Equation  of  Motion 
%*****  Initialize  parameters  and  matrices  ***** 

clc 


ics=[  ]; 
sigmas=[ ); 
error=[  J; 
sensorerror=[  ]; 
ArrayT=[  ]; 
qlhat=[  ]; 
q2hat=[  ]; 
q3hat=[  ]; 
q4hat=[  ]; 
omega  lhat=[  ]; 
omega2hat=[  J; 
omega3hat=[  ]; 
ArraySPl  1=[]; 
Array  SP1 1P=[  ]; 
ArraySP22=[  ]; 
ArraySP22P=[  J; 
ArraySP33=[  ]; 
ArraySP33P=[ ); 
ArraySP44=[  ]; 
ArraySP44P=[ }; 
ArraySP55=(  ]; 


^initialize  error  multiplier 
%initialize  eiror  multiplier 
^initialize  Arrays 
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ArraySP55P=[  ]; 

Array  SP66=[  ]; 

ArraySP66P=[  ]; 

ArraySP77=[  ]; 

ArraySP77P=[  ]; 
qukf=[  ]; 
ukfvec=[  ]; 
statesize=7; 

PHIS=0; 

Q=zeros(statesize,statesize); 

P=zeros(statesize,statesize); 

HMAT=zeros(statesize,statesize); 


%system  order 

%process  noise  (measure  of  uncertainty  in  dynamic  model) 
%initialize  process  noise  matrix 
%initialize  covariance  matrix 
%initialize  linearized  measurement  matrix 


%*****  Assign  initial  estimates  for  omegalhat,  omega2hat,  omega3hat,  qlhat,  q2hat,  q3hat,  q4hat  ***** 

'Do  you  wish  to  enter  initial  estimates  of  the  rotational  body  rates  and  the  quaternion  components' 

'or  let  the  program  use  the  actual  initial  conditions  with  a  user  defined  percent  error  to’ 

’make  an  initial  estimate  (default  is  to  let  the  program  do  it)?' 

’If  you  choose  manual  entry,  an  uncertainty  in  the  estimate  is  also  required.  The  default  is  "auto’V 

ics=input('Enter  'V  for  manual  entry  and  "0”  for  automatic:  '); 
if  isempty(ics) 
ics=0; 
end 


if  ics  =  0 

%*****  base  initialization  on  actual  intial  conditions  with  user  defined  standard  error  ***** 
clc 

error=input('Enter  percentage  error  (+/-)  in  initial  conditions  (i.e.,  -5.2%  =  "-5.2")  Default  is  5%:  ’); 
if  isempty(error) 
error=5 ; 
end 

'percent  error  applied  to  the  initial  conditions  is:  \error 
error=error/100; 

omega  1  hat(  1  )=omega  1  ( 1  )+omega  1  ( 1  )*error; 

omega2hat(  1  )=omega2(  1  )+omega2(  1  )*error; 
omega3hat(l  )=omega3(l  )+omega3(  1  )*error; 
qlhat(l)=ql(l)+ql(l)*error; 


q2hat(  1  )=q2(  1  )+q2(  1  )*error; 
q3hat(  1  )=q3  ( 1  Hq3(  1  )*ertor; 
q4hat(  1  )=q4(  1  >fq4(  1 )  *error ; 
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'apriori  estimate  of  omega  1  (rad/s):  \omegalhat(l) 
'apriori  estimate  of  omega2  (rad/s):  \omega2hat(l) 
'apriori  estimate  of  omega3  (rad/s):  \omega3hat(l) 
'apriori  estimate  of  ql:  \qlhat(l) 

'apriori  estimate  of  q2:  \q2hat(l) 

'apriori  estimate  of  q3:  \q3hat(l) 

'apriori  estimate  of  q4:  \q4hat(l) 


%apriori  estimate  of  omega  1 
%apriori  estimate  of  omega2 
%apriori  estimate  of  omega3 
%apriori  estimate  of  ql 
%apriori  estimate  of  q2 
%apriori  estimate  of  q3 
%apriori  estimate  of  q4 


%*****  Non-zero  entries  of  initial  covariance  matrix  for  auto  initial  cond  ***** 


P(  1 , 1  )=(omega  1  ( 1  )*error+eps)A2; 
P(2,2)=(omega2(  1  )*error+eps)A2 ; 
P(3,3)=(omega3(  1  )*error+eps)A2; 
P(4>4)=(q  l  ( 1  )*error+eps)  A2; 
P(5,5)=(q2(l)*error+eps)A2; 
P(6,6)=(q3(  1  )*error+eps)A2; 
P(7,7)=(q4(  1  )*error+eps)A2; 


%set  at  square  of  uncertainty  in  omega  1 
%set  at  square  of  uncertainty  in  omega2 
%set  at  square  of  uncertainty  in  omega3 
%set  at  square  of  uncertainty  in  ql 
%set  at  square  of  uncertainty  in  q2 
%set  at  square  of  uncertainty  in  q3 
%set  at  square  of  uncertainty  in  q4 


else 


%...or  can  manually  enter  whatever  intialization  values  you  wish... 


omega  lhat(l)=input('Please  enter  initial  omega  1  in  ipm:  '); 
omega2hat(l)=input(*Please  enter  initial  omega2  in  ipm:  '); 
omega3hat(l)=input('Please  enter  initial  omega3  in  rpm:  '); 
qlhat(l)=input('Please  enter  initial  ql:  '); 
q2hat(l)=input('Please  enter  initial  q2:  ’); 
q3hat(  1  )=input('Please  enter  initial  q3:  '); 
q4hat(  1  )=input('Please  enter  initial  q4:  '); 


%apriori  estimate  of  omega  1 
%apriori  estimate  of  omega2 
%apriori  estimate  of  omega3 
%apriori  estimate  of  ql 
%apriori  estimate  of  q2 
%apriori  estimate  of  q3 
%apriori  estimate  of  q4 


omega  1  hat(  1  )=omega  1  hat(  1 )  *2*pi/60; 
omega2hat(  1  )=omega2hat(  1  )*2*pi/60; 

omega3hat(l)=omega3hat(l)*2*pi/60;  %convert  manual  omega  entries  to  rad/s 

'For  manual  entry  of  initial  conditions,  estimated  uncertainty  in  the  initial  conditions' 

’is  necessary  to  intialize  the  covariance  matrix...' 

domegal  hat=input('Please  enter  estimated  uncertainty  in  initial  omega  1  (rad/s):  ');  %uncertainty  in  initial  estimate  of  omega  1 

domega2hat=input('Please  enter  estimated  uncertainty  in  initial  omega2  (rad/s):  ');  ^uncertainty  in  initial  estimate  of  omega2 

domega3hat=input(’Please  enter  estimated  uncertainty  in  initial  omega3  (rad/s):  ');  ^uncertainty  in  initial  estimate  of  omega3 
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dqlhat=input('Please  enter  estimated  uncertainty  in  initial  ql :  ') 
dq2hat=input('Please  enter  estimated  uncertainty  in  initial  q2:  ') 
dq3hat=input('Please  enter  estimated  uncertainty  in  initial  q3:  ') 
dq4hat=input(’Please  enter  estimated  uncertainty  in  initial  q4:  ') 

%*****  Non-zero  entries  of  initial  covariance  matrix 

P(  1 , 1  )=(domega  l  hat+eps)  A2; 

P(2,2)=(domega2hat+eps)A2; 

P(3,3)=(domega3hat+eps)A2; 

P(4,4)=(dq  1  hat+eps)  A2 ; 

P(5,5)=(dq2hat+eps)A2; 

P(6,6)=(dq3hat+eps)A2; 

P(7,7)=(dq4hat+eps)A2; 

end 


%uncertainty  in  initial  estimate  of  ql 
%  uncertainty  in  initial  estimate  of  q2 
^uncertainty  in  initial  estimate  of  q3 
^uncertainty  in  initial  estimate  of  q4 

initial  cond  ***** 

%set  at  square  of  uncertainty  in  initial  estimate  of  omega  1 
%set  at  square  of  uncertainty  in  initial  estimate  of  omega2 
%set  at  square  of  uncertainty  in  initial  estimate  of  omega3 
%set  at  square  of  uncertainty  in  initial  estimate  of  ql 
%set  at  square  of  uncertainty  in  initial  estimate  of  q2 
%set  at  square  of  uncertainty  in  initial  estimate  of  q3 
%set  at  square  of  uncertainty  in  initial  estimate  of  q4 


%insure  initial  quaternion  estimate  is  a  unit  quaternion 

mag=sqrt(q  1  hat(  1 )  A2+q2hat(  1  )A2+q3hat(  1  )A2+q4hat(l  )A2); 

q  1  hat(  1  )=q  1  hat(  1  )/mag ; 

q2hat(  1  )=q2hat(  1  )/mag; 

q3hat(  1  )=q3hat(  1  )/mag; 

q4hat(  1  )=q4hat(  1  )/mag; 

%*****  Enter  measurement  noise  values  for  use  in  calculating  Measurement  Noise  Matrix,  R  ***** 

’Do  you  wish  to  enter  sensor  measurement  sigmas  manually  or  let  the  program  use  the  actual' 

'sensor  measurement  sigmas  with  a  user  defined  percent  error  to  make  an  initial  estimate* 

'(default  is  to  let  the  program  do  it)?' 

sigmas=input('Enter "  1 "  for  manual  entry  and  "0"  for  automatic:  *); 
if  isempty(sigmas) 
sigmas^O; 
end 
clc 

if  sigmas  =  0 

%*****  base  sensor  noise  matrix  on  actual  sigmas  plus  user  defined  error  ***** 

sensorerror=input('Enter  percentage  error  (+/-)  in  sensor  measurement  sigmas  (i.e.,  -5.2%  =  "-5.2")  Default  is  0%:  ’); 
if  isempty(sensorerror) 
sensorerror=0; 


end 
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'percent  error  applied  to  the  actual  sensor  measurement  sigmas  is:  \sensorerror 
sensorerror=sensorerror/l  00; 


SIGOMEGAl=stdomegalmeas;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

SIGOMEGA2=stdomega2meas;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

SIGOMEGA3=stdomega3meas;  %right  now... calculated  in  attitude_filter  from  std  of  actual  error 

estS  IGOM  EGA  1  =SIGOMEG  A 1 +S IGOMEG  A 1  *sensorerror; 

estS  IGOMEG  A2=S IGOMEG  A2+S  IGOMEG  A2*sensorerror; 

estSIGOMEGA3=SIGOMEGA3+SIGOMEGA3*sensorerror; 

estS  IGS  UNX=S  IGSUNX+S IGS  UNX*sensorerror; 

estSIGSUNY=SIGSUNY+SIGSUNY*sensorerror; 

estSIGSUNZ=SIGSUNZ+SIGSUNZ*sensorerror; 

estS  IGM  AGX=S  IGM  AGX+S IGM  AGX  *sensorerror; 

estS  IGM  AG  Y =S  IGM  AG  Y+S  IGM  AG  Y  *sensorerror; 

estS  IGM  AGZ=S  IGM  AGZ+S  IGM  AGZ*sensorerror; 


’apriori  estimate  of  omega  1  "measurement”  sigma  (rad/s):  \estSIGOMEGAl 
'apriori  estimate  of  omega2  "measurement”  sigma  (rad/s):  \estS  IGOMEG  A2 
'apriori  estimate  of  omega3  "measurement"  sigma  (rad/s):  \estS  IGOMEG  A3 
'apriori  estimate  of  sun  sensor  x  axis  measurement  sigma  (deg):  \estSIGSUNX*  180/pi 
’apriori  estimate  of  sun  sensor  y  axis  measurement  sigma  (deg):  \estSIGSUNY*  180/pi 
'apriori  estimate  of  sun  sensor  z  axis  measurement  sigma  (deg):  \estSIGSUNZ*  180/pi 
'apriori  estimate  of  mag  field  sensor  x  axis  measurement  sigma  (deg):  \estS IGM  AGX*  180/pi 
’apriori  estimate  of  mag  field  sensor  y  axis  measurement  sigma  (deg):  \estSIGMAGY*  180/pi 
’apriori  estimate  of  mag  field  sensor  z  axis  measurement  sigma  (deg):  \estSIGMAGZ*  180/pi 


%apriori  estimate  of  SIGOMEGA1 
%apriori  estimate  of  SIGOMEGA2 
%apriori  estimate  of  S  IGOMEG  A3 
%apriori  estimate  of  SIGSUNX 
%apriori  estimate  of  SIGSUNY 
%apriori  estimate  of  SIGSUNZ 
%apriori  estimate  of  S  IGM  AGX 
%apriori  estimate  of  SIGMAGY 
%apriori  estimate  of  SIGMAGZ 


else 


%...or  can  manually  enter  whatever  intialization  values  you  wish... 


estS  IGOMEG  A  l=input(*PIease  enter  sigma  for  the  omega  1  derived  "measurement”  in  rpm:  ’); 
estS  IGOMEG  A2=input(’PIease  enter  sigma  for  the  omega2  derived  "measurement"  in  rpm:  '); 
estS  IGOMEG  A3=input('Please  enter  sigma  for  the  omega3  derived  "measurement"  in  rpm:  ’); 


%apriori  estimate  of  S  IGOMEG  A 1 
%apriori  estimate  of  SIGOMEGA2 
%apriori  estimate  of  S  IGOMEG  A3 


estS  IGOMEG  A  l  =estS  IGOMEG  A  l  *2*pi/60; 
estSIGOMEG  A2=estS  IGOMEG  A2*2*pi/60; 
estSIGOMEGA3=estSIGOMEGA3*2*pi/60; 


%convert  estSIGOMEGAl  to  rad/s 
%convert  estSIGOMEGA2  to  rad/s 
%convert  estSIGOMEGA3  to  rad/s 


estS  IGS UNX=input('PIease  enter  sigma  for  the  sun  sensor  x  axis  measurement  in  deg:  '); 
estSIGSUNY=input('PIease  enter  sigma  for  the  sun  sensor  y  axis  measurement  in  deg:  '); 
estSIGSUNZ=input('PIease  enter  sigma  for  the  sun  sensor  z  axis  measurement  in  deg:  '); 
estS  IGM  AGX=input('Please  enter  sigma  for  the  mag  field  sensor  x  axis  measurement  in  deg: 


%apriori  estimate  of  SIGSUNX 
%apriori  estimate  of  SIGSUNY 
%  apriori  estimate  of  SIGSUNZ 
*);%apriori  estimate  of  SIGMAGX 
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estSIGM  AGY=input('PIease  enter  sigma  for  the  mag  field  sensor  y  axis  measurement  in  deg:  ’);%apriori  estimate  of  SIGM  AGY 
estSIGM  AGZ=input(’Please  enter  sigma  for  the  mag  field  sensor  z  axis  measurement  in  deg:  *);%apriori  estimate  of  SIGMAGZ 


estSIGSUNX=estSIGSUNX*pi/180; 

estSIGSUNY=estSIGSUNY*pi/180; 

estSIGSUNZ=estSIGSUNZ*pi/180; 

estSIGMAGX=estSIGMAGX*pi/180; 

estSIGMAGY=estSIGMAGY*pi/180; 

estSIGM  AGZ=estSIGMAGZ*pi/l  80; 

end 


%convert  from  degrees  to  radians 
^convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 
^convert  from  degrees  to  radians 
%  convert  from  degrees  to  radians 
%convert  from  degrees  to  radians 


'Please  press  "return"  to  continue...' 
pause;clc;close; 

%*****  Non-zero  submatrices  of  Measurement  Noise  Matrix,  R  ***** 


ROMEGA=diag([estSIGOMEG A 1  A2+eps  estSIGOMEGA2A2+eps  estSIGOMEGA3A2+eps]); 

RSENSOR=diag([estSIGSUNXA2+eps  estSIGSUNYA2+eps  estSIGSUNZA2+eps  estSIGMAGXA2+eps  estSIGMAGYA2+eps 
estS  IGM  AGZA2+eps] ) ; 

%*****  user  input  of  process  noise  to  reflect  uncertainty  in  dynamic  model,  PHIS  ***** 

confidence=input('Enter  process  noise  to  reflect  uncertainty  in  model  (default  is  .023):  '); 

if  isempty(confidence) 
conftdence=.023; 
end 


PHIS=confidence;  .  %(100-confidence)/100;  %[17]pg.  317 

'Process  noise  applied  is:  ',PHIS 

for  i=l:l:statesize; 

Q(i,i)=PHISA2; 

end 


%*****  time  constants  and  noise  variances  for  noise  driving  motion  ***** 

'enter  time  constants  associated  with  motion...' 


tauomegal  =  input(*enter  tau  omega l  (default=.34)  '); 
if  isempty(tauomegal ) 
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tauomegal=.34 

end 

tauomega2  =  input('enter  tau  omega2  (default  =  .34)  ’); 
if  isempty(tauomega2) 
tauomega2=.34 
end 

tauomega3  =  input('enter  tau  omega3  (default=le7)  '); 
if  isempty(tauomega3) 
tauomega3=le7 
end 

'press  "return"  to  continue...’ 
pause;clc;dose; 

'Enter  the  scaling  parameters  for  the  Unscented  Transform...' 

alpha=input(*enter  alpha  (default  is  .01):  ’);  %determines  spread  of  sigma  values  about  mean,  .0001<alpha<l 

if  isempty  (alpha) 
alpha=.01 
end 

beta=input('enter  beta  (default  is  2):  ');  %incorporates  prior  knowledge  of  state  dist,  beta=2  for  Gaussian 

if  isempty  (beta) 
beta=2 
end 

augalpha=input('enter  alpha  for  augmented  sigma  (default  is  .01):  '); 
if  isempty  (augalpha) 
augalpha=.01 
end 

%determines  spread  of  sigma  values  about  mean,  .0001<alpha<l 


augbeta=input('enter  beta  for  augmented  sigma  (default  is  2):  '); 
if  isempty  (augbeta) 
augbeta=2 
end 

%incorporates  prior  knowledge  of  state  dist,  beta=2  for  Gaussian 


'Please  wait  while  data  is  processed...' 


%*****  j00p  t0  catcuiate  estimated  states  at  each  time  step  ***** 


count=l; 


%  initialize  counter 


t=0.; 

ArrayT(l)=t; 

while  count<=length(omegalmeas)-l 


^initialize  time 


%*****  Assign  variables  to  more  managable  variable  names  ***** 

x  l=omega  1  hat(count); 
x2=omega2hat(count); 
x3=omega3hat(count); 
x4=qlhat(count); 
x5=q2hat(count); 
x6=q3hat(count); 
x7=q4hat(count); 

den=sqrt(x4A2+x5A2+x6A2+x7A2); 
den3=denA3; 

f4=(xl*x7-x2*x6+x3*x5); 
f5=(xl  *x6+x2*x7-x3*x4); 
f6=(-xl*x5+x2*x4+x3*x7); 
f7=(-xl  *x4-x2*x5-x3*x6); 


%convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 
^convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 
%convenient  compilation  of  terms  to  be  used  later 


%*****  Current  estimate  of  the  state  vector  ***** 


eststate=[xl  x2  x3  x4  x5  x6  x7]'; 

%*****  Calculate  parameter  values  ***** 

Instates  ize; 

kappa=3-L; 

lambda=alphaA2*(L+kappa)-L; 

gamma=sqit(L+Iambda); 

if  isempty(fmd(eig(P)<0)) 

C=chol(P); 

else 

C=zeros(statesize,statesize); 

end 


%dimension  of  state  vector 

%secondary  scaling  parameter 
^primary  scaling  parameter 

%check  for  positive-definiteness  of  P 

%Cholesky  factorization  of  covariance  matrix 


%*****  Calculate  Sigma  Point  Matrix,  sigma  ***** 
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sigma=[  ]; 

sigma(:,l)=eststate;  %Sigma-zero 

for  i=2:l:L+l 

sigma(:,i)==sigma(:,  l)+gamma*C(i-l,:)’;  %Sigma  points  1  through  L  are  eststate  plus  gamma  times  the 

%transpose  of  the  ith  row  of  the  Cholesky  upper  triangular 
^factorization 
end 

for  i=L+2:l:2*L+l 

sigma(:,i)=sigma(:,  l)-gamma*C(i-(L+l %Sigma  points  Lfl  through  2L  are  eststate  minus  gamma 

%times  the  transpose  of  the  ith  row  of  the  Cholesky  upper 
%triangular  factorization 
end 


%*****  Instantiate  Sigma  Points  Through  Nonlinear  Function,  sigmabar  ***** 
%*****  Propagate  estimated  states  forward  using  one-step  Euler  integration  ***** 


sigmabar=[  1; 

for  j= 1 : 1  :Iength(sigma(  1 ,:)) 

Xl=sigma(l  j); 
X2=sigma(2j); 
X3=sigma(3j); 
X4=sigma(4,j); 
X5=sigma(5,j); 
X6=sigma(6,j); 
X7=sigma(7j); 


%omegalhat  component  to  be  propagated  forward  one  time  step 
%omega2hat  component  to  be  propagated  forward  one  time  step 
%omega3hat  component  to  be  propagated  forward  one  time  step 
%qlhat  component  to  be  propagated  forward  one  time  step 
%q2hat  component  to  be  propagated  forward  one  time  step 
%q3hat  component  to  be  propagated  forward  one  time  step 
%q4hat  component  to  be  propagated  forward  one  time  step 


S=0; 

Hl=.01; 


%  initialize  integration  timer 

%set  step  size  for  numerical  integration 


while  S<=(TS-,0001) 


%loops  from  current  step  to  just  short  of  next  time  step 


Xldot=-  1/tauomegal  *X  1 ; 
Xl=Xl+Hl*Xldot; 


X2dot=- l/tauomega2*X2; 
X2=X2+H1  *X2dot; 


X3dot=-l/tauomega3*X3; 
X3=X3+H1  *X3dot; 


X4dot=.5*f4/den; 
X4=X4+H1  *X4dot; 


X5dot=.5*f5/den; 

X5=X5+H1  *X5dot; 

X6dot=.5*f6/den; 

X6=X6+H  1  *X6dot; 

X7dot=.  5*f7/den; 

X7=X7+Hl*X7dot; 

S=S+Hl; 

nd 

sigmabar(l  j)=Xl; 
sigmabar(2,j)=X2; 
sigmabar(3,j)=X3; 
sigmabar(4j)=X4; 
sigmabar(5j)=X5; 
sigmabar(6j)=X6; 
sigmabar(7j)=X7; 

end 

%*****  Compute  the  predicted  mean  of  each  state  ***** 

WmO=Iambda/(L+lambda); 

Wm=  l/(2*(L+lambda)); 

X 1  bar=WmO*sigmabar(  1,1); 

X2bar=WmO*sigmabar(2, 1 ); 

X3bar=WmO*sigmabar(3, 1 ); 

X4bar=WmO*sigmabar(4, 1 ); 

X5bar^=WmO*sigmabar(5, 1 ); 

X6bar=WmO*sigmabar(6, 1 ); 

X7bar=WmO*sigmabar(7, 1 ); 

fork=2:l:2*L+l 

X 1  bar=X  1  bar+W  m*sigmabar(  l  ,k) ; 
X2bar=X2bar+Wm*sigmabar(2,k); 
X3bar=X3bar+Wm*sigmabar(3,k); 

X4bar=X4bar+ W  m*sigmabar(4,k); 
X5bar=X5bar+Wm*sigmabar(5  ,k); 


%  increment  integration  timer  by  defined  integration  step  size 
%yields  each  state  estimate  integrated  forward  to  next  time  step 

%transformed  sigma  points 


%weight  Wm  for  initial  step 
%Wm  for  steps  other  than  zero 


%this  block  predicts  mean  as  a  weighted  sum 
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X6bar=X6bar+Wm*sigmabar(6,k); 

X7bar=X7bar+Wm*sigmabar(7,k); 

end 


statebar=[X  lbar  X2bar  X3bar  X4bar  X5bar  X6bar  X7barj';  %  vector  of  projected  states 

%*****  Compute  the  predicted  covariance  before  the  update,  M  ***** 

WcO=lambda/(L+lambda)+ 1  -alphaA2+beta;  %  weight  Wc  for  initial  step 

Wc=  1/(2  *(L+lambda));  %Wc  for  steps  other  than  zero 

M=WcO*((sigmabar(:,l  )-statebar)*(sigmabar(:,  1  )-statebar)'); 

for  m=2:l:2*L+l 

M=M+Wc*((sigmaba^(:,m)-stateba^)*(sigmaba^(:,m)-stateba^),); 

end 


%*****  Augment  Sigma  point  Matrix  to  Account  for  Process  Noise  Covariance,  Q  ***** 


Lprime=2*L; 

augkappa=3-Lprime; 

auglambda=augalpha/v2*(Lprime+augkappa)-Lprime; 
auggamma=sqrt(Lprime+auglambda); 
if  isempty(fmd(eig(Q)<0)) 

C2=chol(Q); 

else 

C2=zeros(statesi  ze,statesize) ; 
end 

newsigma=sigmabar; 
for  n=l:l:statesize 

newsigma(:,2*L+l+n)=sigmabar(:,l)+auggamma*C2(n,:),; 

end 

for  n=statesize+l :  1 :2*statesize 
newsigmaO^L+l+n^sigmabar^O-auggamma’^fa-L,:)'; 
end 


%augment  with  additional  points 
%secondary  scaling  parameter 
%primary  scaling  parameter 
%gamma  for  augmented  matrix 
%check  for  positive-definiteness 

%Cholesky  factorization  of  process  noise  covariance  matrix 


%*****  Instantiate  transformed  Sigma  points  through  non-linear  observation  model  ***** 

%*****  In  this  implementation,  the  observations  are  the  states  (zetabai^newsigma) !***** 

HMAT=eye(7);  %in  this  implementation,  measurements  are  the  states 


for  p=l :  1  :length(newsigma(l,:)) 
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zetabar(  1  ,p)=newsigma(  1  ,p); 
zetabar(2,p)=newsigma(2,p); 
zetabar(3,p)=newsigma(3,p); 
zetabar(4,p)=newsigma(4,p); 
zetabar(5,p)=newsigma(5,p); 
zetabar(6,p)=newsigma(6,p); 
zetabar(7,p)=newsigma(7,p); 
end 

%*****  Compute  the  predicted  observation  vector,  obsbar  ***** 

augWmO=augIambda/(Lprime+auglambda); 
augWm=l/(2*(Lprime+auglambda)); 

z  lbar=augWmO*zetabar(  1,1); 
z2bar=augWmO*zetabar(2, 1 ); 
z3bar=augWm0*zetabar(3, 1 ); 
z4bar=augWmO*zetabar(4, 1 ); 
z5bar=augWm0*zetabar(5, 1 ); 
z6bar=augWmO*zetabar(6(  1 ); 
z7bar=augWmO*zetabar(7, 1 ); 

for  k=2: 1  :length(newsigma(l,:)) 
z  1  bar=z  1  bar+augW  m*  zetabar(  1  ,k) ; 
z2bar==z2bar+augWm*zetabar(2,k); 
z3bar=z3bar+augWm*zetabar(3,k); 
z4bar=z4bar+augWm*zetabar(4,k); 
z5bar=z5bar+augWm*zetabar(5,k); 
z6bar=z6bar+augWm*zetabar(6,k); 
z7bar=z7bar+augWm*zetabar(7,k); 
end 

obsbar=[zlbar  z2bar  z3bar  z4bar  z5bar  z6bar  z7bar]';  %  vector  of  projected  observations 

%*****  Calculate  Measurement  Noise  Matrix,  RMAT  ***** 

%*****  Uses  A  matrix  from  Gauss_newton  to  relate  6x6  RSENSOR  to  covariance  of  quaternions  after  convergence  [1 1]  ****** 
RMAT=[ROMEGA,  zeros(3,4);zeros(4,3),  A(:,:,count)*RSENSOR*A(:,:, count)’]; 

%*****  Calculate  Innovation  Covariance  Matrix,  Pzz  ***** 


%  weight  Wm  for  initial  step 
%Wm  for  steps  other  than  zero 


%this  block  predicts  mean  as  a  weighted  sum 


augWcO=auglambda/(Lprime+auglambda)+ 1  -auga!phaA2+augbeta; 
augWc=l/(2*(Lprime+auglambda)); 

Pzz=augWcO*((zetabar(: ,  1  )-obsbar)*(zetabar(:,  1  )-obsbar)’); 


%wcight  Wc  for  initial  step 
%Wc  for  steps  other  than  zero 
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for  r=2:l:2*Lprime+l 

Pzz=Pzz+augWc*((zetabar(:,r)-obsbar)*(zetabar(:,r)-obsbary); 

end 

Pzz=Pzz+RMAT; 

%*****  Calculate  Cross-Correlation  Matrix,  Pxz  ***** 
Pxz=augWcO*((newsigma(:,  1  Vstatebar^zetabar^U-obsbar)'); 
for  r=2:l:2  *Lprime+ 1 

Pxz=Pxz+augWc*((newsigma(:,^)-stateba^)*(zetabar(:,r)-obsba^),); 

end 


%*****  Calculate  Kalman  Gain  Matrix  ***** 


K=Pxz*inv(Pzz);  %calculate  Kalman  gain  matrix 

%*****  Form  observation  vector  (measurement)  ***** 

obs(l  :7,count+l)=[omegalmeas(count+l)  omega2meas(count+l)  omega3meas(count+l)  q_min(l,count+l)  q_min(2,count+l) 
q_min(3,count+ 1 )  q_min(4,count+ 1 )] 

%*****  Calculate  updated  state  estimates  using  propagated  states,  residuals  and  Kalman  gains 

residual=(obsbar-obs(  1 :7,count+ 1 )); 
statehat=statebar-K*residual ; 

%*****  Calculate  Covariance  After  the  Update  ***** 

P=M-K*Pzz*K';  %ca!cuate  covariance  after  update 

P=sqrt(P.A2); 


%*****  Increment  time  ***** 


t=t+TS; 


^increment  time 


%*****  Check  for  loss  of  data  ***** 


if  q_min(l:4,count)=[0  0  0  1]'; 

omega  lhat(count+l  )=omegal  hat(count); 
omega2hat(count+ 1  )=omega2hat(count) ; 
omega3hat(count+ 1  )=omega3  hat(count) ; 
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q  1  hat(count+ 1  )=q  1  hat(count); 
q2hat(count+l  )=q2hat(count); 
q3hat(count+ 1  )=q3hat(count); 
q4hat(count+l)=q4hat(count); 
flag(count)=l; 

else 

%***************  Data  management  ***************** 

omega  lhat(count+l)=statehat(l); 

omega2hat(count+l  )=statehat(2); 

omega3hat(count+l)=statehat(3); 

q  1  hat(count+ 1  )=statehat(4); 

q2hat(count+ 1  )=statehat(5) ; 

q3hat(count+ 1  )=statehat(6) ; 

q4hat(count+l  )=statehat(7); 

end 

%**************  Calculate  Covariance  of  the  Residual  **************** 


covRES=HMAT*M*HMAT'+RMAT; 

ArrayT(count+ 1  )=t; 

%***************  Error  Analysis  ****************** 

SPll=sqrt(P(l,l)); 

SP11P=-SP11; 

SP22=sqrt(P(2,2»; 

SP22P=-SP22; 

SP3  3=sqrt(P(3 ,3)); 

SP33P=-SP33; 

SP44=sqrt(P(4,4»; 

SP44P=-SP44; 

SP55=sqrt(P(5,5)); 

SP55P=-SP55; 

SP66=sqrt(P(6,6)); 

SP66P=-SP66; 

SP7  7 =sqrt(P(7 ,7 )) ; 

SP77P=-SP77; 

ArrayS  PI  l(count+l)=SPl  1; 

ArraySP  1 1  P(count+ 1  )=SP  1 1 P; 

ArrayS  P22(count+ 1  )=SP22; 


^Calculate  theoretical  upper  error  bound  on  omega  1  hat 
%Calculate  theoretical  lower  error  bound  on  omega  1  hat 
^Calculate  theoretical  upper  error  bound  on  omega2hat 
^Calculate  theoretical  lower  error  bound  on  omega2hat 
^Calculate  theoretical  upper  error  bound  on  omega3hat 
^Calculate  theoretical  lower  error  bound  on  omega3hat 
^Calculate  theoretical  upper  error  bound  on  qlhat 
^Calculate  theoretical  lower  error  bound  on  qlhat 
%  Calculate  theoretical  upper  error  bound  on  q2hat 
^Calculate  theoretical  lower  error  bound  on  q2hat 
%Calculate  theoretical  upper  error  bound  on  q3hat 
^Calculate  theoretical  lower  error  bound  on  q3hat 
%  Calculate  theoretical  upper  error  bound  on  q4hat 
%Calculate  theoretical  lower  error  bound  on  q4hat 
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ArraySP22P(count+ 1  )=SP22P; 
ArrayS  P33(count+ 1  )=SP33; 
ArraySP33P(count+l)=SP33P; 
ArraySP44(count+ 1  )=SP44; 
Array  SP44P(count+  1)=SP44P; 
Array  SP55(count+ 1  )=SP55; 
ArraySP55P(count+l)=SP55P; 
ArraySP66(count+ 1  )=SP66; 
ArraySP66P(count+ 1  )=SP66P; 
ArraySP77(count+ 1  )=SP77 ; 
ArraySP77P(count+l)=SP77P; 


%*************  Residual  test  *************** 


RESSPll=sqrt(covRES(U»; 

RESSP11P=-RESSP1 1; 

RESSP22=sqrt(covRES(2,2)); 

RESSP22P=-RESSP22; 

RESSP33=sqrt(covRES(3,3»; 

RESSP33P=-RESSP33; 

RESSP44=sqrt(covRES(4,4)); 

RESSP44P=-RESSP44; 

RESSP55=sqrt(covRES(5,5)); 

RESSP55P=-RESSP55; 

RES  SP66=sqrt(co  vRES  (6,6)) ; 
RESSP66P=-RESSP66; 
RESSP77=sqrt(covRES(7,7)); 
RESSP77P=-RESSP77; 

ArrayRES  l(count+l  )=residual(  1 ); 
ArrayRES2(count+l)=residual(2); 
ArrayRES3(count+l  )=residual(3); 
ArrayRES4(count+ 1  )=residual(4); 
ArrayRES5(count+l)~residual(5); 
ArrayRES6(count+l  )=residual(6); 
ArrayRES7(count+l)=residual(7); 

ArrayRESSPl  l(count+l)=RESSPl  1; 

ArrayRESSPl  lP(count+l)=RESSPl  IP; 

ArrayRESSP22(count+ 1  )=RESSP22; 

ArrayRESSP22P(count+l)=RESSP22P; 

ArrayRESSP33(count+l)=RESSP33; 

ArrayRESSP33P(count+l)=RESSP33P; 

ArrayRESSP44(count+l)=RESSP44; 


%Calculate  theoretical  upper  error  bound  on  residual  for  omega  1  hat 
^Calculate  theoretical  lower  error  bound  on  residual  for  omega  1  hat 
^Calculate  theoretical  upper  error  bound  on  residual  for  omega2hat 
%CaIculate  theoretical  lower  error  bound  on  residual  for  omega2hat 
%Calculate  theoretical  upper  error  bound  on  residual  for  omega3hat 
%Calculate  theoretical  lower  error  bound  on  residual  for  omega3hat 
%Calculate  theoretical  upper  error  bound  on  residual  for  qlhat 
^Calculate  theoretical  lower  error  bound  on  residual  for  qlhat 
%Calculate  theoretical  upper  error  bound  on  residual  for  q2hat 
%Ca!culate  theoretical  lower  error  bound  on  residual  for  q2hat 
%Calculate  theoretical  upper  error  bound  on  residual  for  q3hat 
%Calculate  theoretical  lower  error  bound  on  residual  for  q3hat 
^Calculate  theoretical  upper  error  bound  on  residual  for  q4hat 
%CalcuIate  theoretical  lower  error  bound  on  residual  for  q4hat 


ArrayRESSP44P(count+l)=RESSP44P; 

ArrayRESSP55(count+l)=RESSP55; 

ArrayRESSP55P(count+l)=RESSP55P; 

ArrayRESSP66(count+l)=RESSP66; 

ArrayRESSP66P(count+l)=RESSP66P; 

ArrayRESSP77(count+l)=RESSP77; 

ArrayRESSP77P(count+l  )=RESSP77P; 

count=count+ 1 ;  %increment  counter 

end 

%******  calculate  mean  square  errors  for  "instantaneous"  and  for  UKF  ****** 

%******  mean  square  error  excludes  first  5  samples  to  reduce  effects  of  transients  ***** 

s2=length  (omega  1  meas); 
sl=5; 


omega  1  meas_mse=mean_square_error(omega  1  meas(s  1  :s2),omega  1  (s  1  :s2)); 
omega2meas_mse=mean_square_error(omega2  meas(s  1  :s2),omega2(s  1  :s2»; 
omega3meas_mse=mean_square_error(omega3meas(sl  :s2),omega3(sl  :s2)); 
q  1  „q_min_mse=mean_square_enror(q„min(  1  ,(s  1  :s2)),q  1  (s  1  :s2)); 
q2_q_min_mse=mean„square_error(q_min(2,(s  1  :s2)),q2(s  1  :s2)); 
q3_q_min_mse=mean_square„error(q_min(3,(sl  :s2)),q3(sl  :s2)); 
q4_q„min_mse=mean_square_error(q_min(4,(sl  :s2)),q4(s  1  :s2)); 

omega  l_ukLmse=mean_square__error(omega  1  hat(s  1  :s2),omega  1  (s  1  :s2)); 
omega2_ukf_mse=mean_square_error(omega2hat(s  1  :s2),omega2(s  1  :s2»; 
omega3„ukf_mse=mean_square_error(omega3hat(sl  :s2),omega3(sl  :s2)); 
q  l_ukf_mse=mean_square_error(q  1  hat(s  1  :s2),q  1  (s  1  :s2)); 
q2_ukf_mse=mean_square_error(q2hat(s  1  :s2),q2(s  1  :s2)); 
q3„ukf„mse=mean„square_error(q3hat(sl  :s2),q3(sl  :s2)); 
q4_ukf_mse=mean_square_etror(q4hat(s  1  :s2),q4(s  1  :s2)); 

%***************  Plotting  and  output  statements  ***************** 

plots=input('Enter  "1"  to  display  plots.  Enter  "0"  for  no  plots.  Default  is  no  plots:  '); 
if  isempty(plots) 
plots=0; 
end 

if  plots=l 

%******  plots  of  error  vs.  true  and  theoretical  error  bounds  ******* 


plot(ArrayT, (omega  1  ( 1  :s2)-omega  lhat),  ArrayT,  ArrayS  PI  1  ,'x',  ArrayT,  ArraySP  1 1  P,'x'); 

grid  on;titIe(’error  in  UKF  estimate  of  omega  1'); 

legend(’error  in  omega  lhat', ’theoretical  bound'); 

xlabel(’time  (sec)');ylabel('error  in  omega  lhat  (rad/s)'); 

pause;close;clc; 

plot(  Array  T,(omega2(l  :s2)-omega2hat),  ArrayT,  ArrayS  P22,’x',  ArrayT,  ArraySP22P,’x'); 

grid  on;title('error  in  UKF  estimate  of  omega2'); 

legend(’error  in  omega2hat’, 'theoretical  bound'); 

xlabel('time  (sec),);ylabel('error  in  omega2hat  (rad/s)’); 

pause;close;c!c; 

plot(AirayT, (omega3(l:s2)-omega3hat), Array  T,ArraySP33,'x’,ArrayT,ArraySP33P,,x'); 

grid  on;title('error  in  UKF  estimate  of  omega3’); 

legend('error  in  omega3  hat', 'theoretical  bound'); 

xlabel(’time  (sec)');yIabel('error  in  omega3hat  (rad/s)’); 

pause;close;clc; 

pIot(ArrayT,(q  1  ( 1  :s2)-ql  hat),  ArrayT,  ArraySP44,'x’,  ArrayT,  ArrayS  P44P/x'); 

grid  on;title('error  in  UKF  estimate  of  ql’); 

legendCerror  in  q lhat’, 'theoretical  bound’); 

xlabel(’time  (sec)');ylabel(’error  in  ql’); 

pause;close;clc; 

pIot(ArrayT,(q2(l;s2)-q2hat),  Array  T,ArraySP55,'x',ArrayT,ArraySP55P,’x'); 

grid  on;title('error  in  UKF  estimate  of  q2’); 

legend(’error  in  q2hat', 'theoretical  bound’); 

xlabel(’time  (sec)’);ylabel(’error  in  q2’); 

pause;close;clc; 

plot(  ArrayT, (q3(  1  :s2)-q3hat),  ArrayT,  ArrayS  P66,'x',  ArrayT,  ArrayS  P66P,’x'); 

grid  on;title('error  in  UKF  estimate  of  q3'); 

legend(’error  in  q3hat’, 'theoretical  bound'); 

xlabel(’time  (sec)');ylabeICerror  in  q3’); 

pause;close;clc; 

plot(ArrayT,(q4(  1  :s2)-q4hat),  ArrayT,  Array  SP77,'x’,  ArrayT,  ArraySP77P,'x’); 

grid  on;title('error  in  UKF  estimate  of  q4'); 

legend(’error  in  q4hat', 'theoretical  bound’); 

xlabel('time  (sec)’);ylabel('error  in  q4’); 

pause;close;clc; 


%******  plots  to  show  true  vs.  quaternion  derived  alone  vs.  UKF  estimates  ******* 
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plot(  ArrayT,  omegal  meas,'gx',  ArrayT,  omegal  hat, ’m\  ArrayT,  omegal(t:s2),'--,);grid  on; 

title('omegal  from  sensor,  UKF estimate  of  omegal,  "True"  omegal' ); 

xlabel('time  (sec)’);ylabel('omegal  (rad/s)'); 

legend(’omegal  from  rate  sensor’, ’omegal  from  UKF,’true  omegal’); 

pause  ;clc;close; 

plot(  ArrayT,  omega2meas,'gx\ ArrayT,  omega2hat,’m\  ArrayT,  omega2(  1  :s2),'-');grid  on; 

titleComega2  from  sensor,  UKF  estimate  of  omega2,  "True"  omega2’ ); 

xlabel('time  (sec)’);ylabelComega2  (rad/s)’); 

legendComega2  from  rate  sensor’, 'omega2  from  UKF,'true  omega2‘); 

pause;clc;close; 

plot(  ArrayT,  omega3meas,’gx',ArrayT,  omega3hat,'m',  ArrayT,  omega3(l:s2),’--');grid  on; 
title(’omega3  from  sensor,  UKF  estimate  of  omega3,  "True"  omega3’ ); 
xlabel(’time  (sec)');yIabelComega3  (rad/s)'); 

Iegend(’omega3  from  rate  sensor', 'omega3  from  UKF,'true  omega3’); 
pause;clc;close; 

%******  plots  of  residual  vs.  theoretical  bounds  on  residual  ******* 

resplots=inputCEnter  "r  to  display  plots  from  residual  test.  Enter  "0"  for  no  plots.  Default  is  no  plots:  ’); 
if  isempty(resplots) 
resplots=0; 
end 

if  resplots=l 

plot(  ArrayT,  ArrayRES  1 ArrayT,  ArrayRESSP  1 1  ,’x',  ArrayT,  ArrayRESSP  1 1  P,'x'); 

grid  on;title('residual  for  omegal'); 

legend('residual  for  omegal’, 'theoretical  bound'); 

xlabel('time  (sec)*);ylabel(’residual  for  omegal  (rad/s)’); 

pause;close;clc; 

plot(ArrayT,ArrayRES2,'-+',ArrayT,AiTayRESSP22,V, ArrayT, ArrayRESSP22P,'x'); 

grid  on;title(’residual  for  omega2’); 

legend('residual  for  omega2','theoretical  bound'); 

xlabel('time  (sec)’);ylabel('residual  for  omega2  (rad/s)’); 

pause;close;clc; 

plot(ArrayT,ArrayRES3, '-4-',  ArrayT,  ArrayRESSP33,'x', ArrayT,  ArrayRESSP33P,’x’); 
grid  on;title('residual  for  omega3'); 
legend(’residual  for  omega3', 'theoretical  bound'); 
xlabel('time  (sec)’);ylabel('residual  for  omega3  (rad/s)’); 


pause;close;clc; 


pIot(ArrayT,ArrayRES4,'-+\ArrayT,ArrayRESSP44,,x\ArrayT,AiTayRESSP44P,,x’); 

grid  on;title('residual  forqr); 

legend(’residual  for  ql’, 'theoretical  bound'); 

xlabel(’time  (sec),);ylabel('residual  forql'); 

pause;c!ose;c!c; 

plot(ArrayT,ArrayRES5,'-+',ArrayT,ArrayRESSP55,'x\ArrayT,ArrayRESSP55P,'x'); 

grid  on;titIe(’residua!  for  q2‘); 

legend('residual  for  q2',’theoretical  bound'); 

xlabel('time  (sec)');ylabel(Tesidual  for  q2'); 

pause;close;clc; 

plot(ArrayT,ArrayRES6,'-+\AiTayT,ArrayRESSP66,,x',AiTayT,ArrayRESSP66P,'x'); 

grid  on;title(*residual  for  q3’); 

legend('residual  for  q3','theoretical  bound’); 

xlabei('time  (sec)*);ylabel('residual  for  q3'); 

pause;c!ose;cIc; 

pIot(ArrayT,ArrayRES7,'-+',ArrayTtArrayRESSP771'x',ArrayT,ArrayRESSP77P,’x'); 

grid  on;title('residua!  for  q4'); 

legend(’residual  for  q4','theoretical  bound’); 

xlabel('time  (sec)');ylabel(,residua!  for  q4  (rad/s)*); 

pause;close;clc; 

end 

end 

'Summary  of  performance  of  each  method...' 

'The  MSE  in  omega  1  from  the  rate  sensor  is  (rad/s):  7  \omegalmeas_mse 

The  MSE  in  omega!  for  the  UKF  estimates  is  (rad/s): ',omegal_ukf_mse 

'The  MSE  in  omega2  derived  from  the  rate  sensor  is  (rad/s):  7  '}omega2meas_mse 

'The  MSE  in  omega2  for  the  UKF  estimates  is  (rad/s):  7  \omega2_ukf_mse 

'Please  press  "return"  to  continue...* 
pause  ;clc;close; 


The  MSE  in  omega3  derived  from  the  rate  sensor  is  (rad/s): \omega3meas_mse 
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The  MSE  in  omega3  for  the  UKF  estimates  is  (rad/s):  V  \omega3_ukf_mse 
'The  MSE  in  ql  derived  using  Gauss-Newton  is:  V  \ql_q_min_mse 
'The  MSE  in  ql  for  the  UKF  estimates  is:  7  \ql_ukf_mse 

1  i 

'Please  press  "return''  to  continue...’ 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.)  ...’ 

The  MSE  in  q2  derived  using  Gauss-Newton  is: \q2_q_min_mse 

The  MSE  in  q2  for  the  UKF  estimates  is: \q2_ukf_mse 

'press  "return"  to  continue...’ 
pause;clc;cIose; 

'Summary  of  performance  of  each  method  (cont.) 

'The  MSE  in  q3  derived  using  Gauss-Newton  is:  7  ',q3_q_min_mse 

'The  MSE  in  q3  for  the  UKF  estimates  is:  7  ',q3_ukf_mse 

'The  MSE  in  q4  derived  using  Gauss-Newton  is:  7  \q4„q_min_jnse 

'The  MSE  in  q4  for  the  UKF  estimates  is:  7  \q4_ukf_mse 

'press  "return"  to  continue...' 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) ...' 

%******  rotate  each  vector  using  actual  and  UKF  attitude  quaternion  ****** 
%******  check  mean-square-error  between  vectors  ****** 


%'******  Evaluate  Mean-Square-Error  of  rotated  vectors  using  Gauss-Newton  derived  quaternion  ******* 
%'Press  "return"  to  rotate  the  reference  vector  at  each  time  step  to  the' 

% 'representation  in  the  rotated  frame  using  both  the  actual  rotation  quaternion' 

%'and  the  quaternion  derived  using  the  UKF...once  rotation  is  complete,' 

%'calculate  the  mean-square-error  between  the  results.' 

%pause;close;clc; 
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qukf=[q  1  hat;q2hat;q3hat;q4hat] ;  %  matrix  representation  of  ukf  quaternions 

%for  j= 1  :length(q  1  hat) 

%  qukf(  1 :4,j)=qukf(  1 :4  j)/norm(qukf(  1 :4  j));  %normalize  to  insure  unit  quaternion 

%end 

ukf_mse_error=[  ]; 
for  j=l:count; 

ukfvec(l  :6,j)=qframerot6(qukf(  1 :4  j),vref(  1 :6  j));  ^rotate  ref  vector  using  min  error  quaternion 

Q_vec(l  :6j)=qframerot6(Qvec(  1 :4  j),  vref(  1 :6  j));  %  rotate  ref  vector  using  true  quaternion 

ukf_mse_error(j)=mean_square_error(ukfvec(l:6,j),Q„vec(l:6j));  %calculate  MSE  between  rotated  vectors 
end 

%*****  determine  angle  between  rotated  vectors  ***** 
for  k=l:count 

oukfsunangle(k)=acos(dot(ukfvec(l  :3,k),Q_vec(l  :3,k))/(norm(ukfvec(l  :3,k))*norm(Q_vec(l  :3,k)))); 
oukfmagangle(k)=acos(dot(ukfvec(4:6,k),Qjvec(4:6,k))/(norm(ukfvec(4:6,k))*norm(Q_vec(4:6,k)))); 
oGNsunangle(k)=acos(dot(q_minvec(  1 :3,k),Q_vec(l  :3,k))/(norm(q_minvec(l  :3,k))*norm(Q_vec(l  :3,k)))); 
oGNmagangle(k)=acos(dot(q_minvec(4:6,k)1Q__vec(4:6,k))/(norm(q_minvec(4:6,k))*norm(Q_vec(4:6,k)))); 
end 

oukfsunangle-oukfsunangle*  1 80/pi; 
oukfmagangle=oukfmagangle*  1 80/pi; 
oGNsunangle=oGNsunang!e*  1 80/pi; 
oGNmagangle=oGNmagangle*  1 80/pi; 

%******  Plot  vectors  rotated  using  derived  and  actual  quaternions  ***** 
pIot3(Q_vec(  1, 1  :count),Q_vec(2, 1  :count),Q_vec(3, 1  :count),,b,);grid  on;hold  on; 
plot3(ukfvec(l ,  1  :count),ukfvec(2, 1  :count),ukfvec(3, 1  ;count),'m'); 
plot3  (Q_vec(4>  1  :count),Q_vec(5, 1  :count),Q_vec(6, 1  :count),V);grid  on;hold  on; 
plot3(ukfvec(4, 1  :count),ukfvec(5, 1  :count),ukfvec(6, 1  ;count),’g'); 
xlabeI('X  component')  ;ylabeI('Y  component’)  ;zlabelCZ  component’); 

legend(’sun  vector,  actual  q\'sun  vector,  ukf  qVmag  field  vector,  actual  q’,  'mag  field  vector,  ukf  q'); 
title('3-D  plot  of  reference  vectors  rotated  using  actual  quaternions  and  ukf  derived  quaternions'); 
pause;c!ose;clc; 

%******  Plot  mean-square-error  between  reference  vector  rotated  using  actual  quaternion  ****** 

%******  and  reference  vector  rotated  using  derived  quaternion.  ****** 

plot(T(l  :count),ukf„mse_error(l  :count)),grid  on; 

title(’MSE  between  reference  vectors  rotated  using  ukf  derived  and  actual  quaternions'); 
xlabel(*time  (sec)');ylabel(,MSE,);legend('MSE') 

'press  "return"  to  continue...' 
pause;close;clc; 


**********************  Overall  Performance  Indicator  ************************' 

*******  Excludes  first  five  measurements  to  reduce  effect  of  transients  ******* 

The  average  of  the  omega  mean-square-errors  for  the  differencing  method  is:  ' 

GNomegaperf=(omega  1  meas_mse+omega2meas_mse+omega3meas_mse)/3 

The  average  of  the  omega  mean-square-errors  for  the  ukf  results  is:  ' 
ukfomegaperf=(omegal_ukf_mse+omega2_ukf_mse+omega3_ukf_mse)/3 

'The  average  of  the  quaternion  component  mean-square-errors  for  the  Gauss-Newton  method  is:  ' 

GNqperf=(q  1  _q_min_mse+q2_q_min_mse+q3_q_min_mse+q4_q_min_mse)/4 

'The  average  of  the  quaternion  component  mean-square-errors  for  the  ukf  results  is:  * 
ukfqperf-(q  1  _ukf_mse-t-q2_ukf_mse+q3_ukf_mse+q4_ukf_mse)/4 

’press  "return"  to  continue...’ 
pause;clc;c!ose; 

'Summary  of  performance  of  each  method  (cont.)  ...’ 

'The  average  MSE  for  the  ref  vectors  rotated  using  GN  quaternions:  ' 
mean_GN_mse_error=mean  (GN_mse_error) 

’The  average  MSE  for  the  ref  vectors  rotated  using  ukf  quaternions:  ' 
mean_ukf_mse_error=mean(ukf_mse_error) 

'press  "return"  to  continue...’ 
pause;clc;close; 

'Summary  of  performance  of  each  method  (cont.) ...’ 

'overall  GN  performance:  sqrt[(mean  omega  MSE)A2  +  (mean  rotated  vector  MSE)A2)]' 
overall_GN_perf=sqrt(GNomegaperfA2+mean_GN_mse_errorA2) 

’overall  ukf  performance:  sqrt[(mean  omega  MSE)A2  +  (mean  rotated  vector  MSE)A2)]' 
overall_ukf_perf=sqrt(ukfomegaperfA2+mean_ukf_mse_errorA2) 

%»**************  ***************************************  ***************  *********  *********** 
%************** ******** ************  ukf  summary  **** ********************* *************** 
%’***  *****  *****************  **********  ***********************  ****************************  ** 
'omega:  \ukfomegaperf 

’quaternion:  ',mean_ukf_mse_error 
'overall:  ',overall_ukf_perf 
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%’mean  sun  angle  error:  ',mean(oukfsunangle) 

%'std  sun  angle  error:  '.std(oukfsunangle) 

%'max  sun  angle  error:  \max(oukfsunangle) 

%’mean  mag  angle  error:  \mean(oukfmagangIe) 

%’std  mag  angle  error:  \std(oukfmagangle) 

%’max  mag  angle  error:  *,max(oukfmagangle) 

I  l 

'press  "return"  to  continue...' 
pause;close;clc; 

The  next  option  allows  the  calculation  of  "error  angles"  between  the  body  frame  rotated  using* 

'the  true  attitude  quaternion  and  using  the  filter  estimated  quaternion.  This  option  also  allows' 

'3D  depiction  of  the  body  frame,  in  inertial  cooridnates,  over  time.  It  can  take  a  while  to  generate.' 

I  1 

angles=input('Enter  "  1 "  to  calculate  attitude  error  angles.  Enter  "0"  for  no  angles.  Default  is  no  angles:  '); 
if  isempty(angles) 
angles=0; 
end 

if  angles— 1 

^Calculate  angles  between  body  frame  rotated  using  estimated  quaternion  and 
%rotated  using  "true"  quaternion 

[ovecl  ,ovec2,ovec3,ovecl  hat,ovec2hat,ovec3hat,angl  ,ang2,ang3]=orientation3(Qvec,  qukf); 

plot(ArrayT,angl);grid  on;title('Error  Angle  For  Axis  r);xlabel('time  (sec)');ylabel(' Angle  1  (deg)'); 
pause;close;clc; 

plot(ArrayT(2:Iength(ang2)),ang2(2:length(ang2)));grid  on;title(’Error  Angle  For  Axis  2,);xlabel('time  (sec)');ylabel(’ Angle  2 
(deg)'); 

pause;close;clc; 

plot(ArrayT,ang3);grid  on;title('Error  Angle  For  Axis  3');xlabelCtime  (sec)');ylabel(’ Angle  3  (deg)*); 
pause;close;clc; 

end 

A.17  Measadjust.m 

This  script  takes  attitude__sim.m  generated  data  and  allows  a  user  to  specify  a  different  measurement 
interval.  It  initializes  the  appropriate  arrays  and  variables,  performs  the  Gauss-Newton  error  minimization 
step,  and  allows  the  m-files  with  the  filtering  algorithms  to  be  run  directly,  on  the  reduced  set  of 


measurements. 


%Measadjust  -  this  script  decreases  number  of  measurements  available  for 
%  filtering.  Prompts  user  for  the  measurement  interval  desired,  and  then 
^extracts  these  measurements  from  the  original  set  generated  by 
%attitude_sim.m.  Reinitializes  the  appropriate  variables  and  arrays  to 
%allow  successful  processing  of  the  reduced  measurement  set.  Performs 
%Gauss-Newton  error  minimization  step.  User  must  then  run  norates_ekf.m, 
%norates_ukf.m,  rates_ekf.m  or  rates_ukf.m  independently  to  process  reduced 
%measurement  set. 

% 

%  Written  by  Mark  Charlton.  Last  updated  3 1  October  2003. 

clc 

TSold=TS; 

countold=count; 

TS=input('Please  enter  desired  interval  for  measurements  in  seconds:  *) 

if  isempty  (TS) 

TS=TSold; 

end 

countstep-ceil(TS/TSold); 

count=ceiI(count/countstep); 

sunmeas=sunmeas(l  :3,1  :countstep:countold); 
magmeas=magmeas(l  :3, 1  :countstep:countold); 

SUNVEC=SUNVEC(  1 :3, 1  :countstep:countold); 

MAGVEC=MAGVEC(1 :3, 1  :countstep:countold); 
omega  1  meas=omega  1  meas(  1  xountstepxountold); 
omega2meas=omega2meas(  1  :countstep:countold); 
omega3meas=omega3meas(  1  :countstep:countold); 
omega  1  =omega  1  ( 1  :countstep:countold); 
omega2=omega2(  1  :countstep:countold); 
omega3=omega3(l  xountstepxountold); 

T=T(  1  :countstep:countold); 
omega  1  _q_min=[  ] ; 
omega2__q_min-[  ]; 
omega3_q_min=[  ]; 

Qvec=Qvec(  1 :4, 1  xountstepxountold); 

Q_refvec=[  ]; 
q_minvec=[  ]; 


ql=ql  (1  xountstepxountold); 
q2=q2(l  xountstepxountold); 
q3=q3(l  xountstepxountold); 
q4=q4(l  xountstepxountold); 


*******  Gauss-Newton  error  minimization  ******* 

’Press  "return”  to  calculate  the  attitude  quaternion  from  the  noisy* 

'attitude  measurements  using  a  Gauss-Newton  error  minimization...' 
pause;close;clc; 

vref=[  ]; 
vrot=[]; 
q_min=[]; 
q_min_dot=[  ]; 

qmin  1  init=0;  %initial  estimate  of  attitude  quaternion 

qmin2init=0; 

qmin3init=0; 

qmin4init=l; 

A=[]; 
check=[  ]; 
err^H; 

for  i=lxount; 

vref(l  :6,i)=[SUNVEC(l:3,i);MAGVEC(l:3ti)];  %form  reference  vector  from  known  inertial  vectors 

if  i<2 

q_init=[qminlinit  qmin2init  qmin3init  qmin4init]'; 
else 

q_init=q_min(  1 :4,i- 1 ); 
end 

if  norm(sunmeas(  1 :3,i))<.0 1  ^default  values  in  case  of  loss  of  data  (no  sun  vector  measurement) 

if  i=l 

q_min(l:4,i)=[qminlinit  qmin2init  qmin3init  qmin4init]'; 
else 

q_min(l  :4,i)=qukf(l  :4,i-l); 
end 

A(1  ;4, 1 :6,i)=A(l  :4, 1 :6,i-l); 
check(i)=l; 
err(i)=l; 


%check  -  1  indicates  missing  measurement 
%err  =  1  indicates  missing  measurement; 


omega  1  meas(i)=omega  1  hat(i- 1 ); 
omega2meas(i)=omega2hat(i- 1 ); 
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omega3meas(i)=omega3hat(i- 1 ); 

elseif  norm(magmeas(l  :3,i))<.0l  %default  values  in  case  of  loss  of  data  (no  mag  field  measurement) 

if  i=l 

q„min(l  :4,i)=[qminlinit  qmin2init  qmin3init  qmin4init]'; 
else 

q_min(  1 :4,i)=qukf(  1 :4,i- 1 ); 
end 

A(  1 :4tl  :6,i)=A(l  :4, 1 :6»i-l); 

check(i)=l;  %check  =  1  indicates  missing  measurement 

err(i)= ! ;  %err  =  1  indicates  missing  measurement; 

omega  1  meas(i)=omega  1  hat(i- 1 ); 
omega2meas(i  )=omega2hat(i- 1 ); 
omega3meas(i)=omega3hat(i- 1 ); 


else 

%if  measurements  exist... proceed  with  minimization... 

unitsunmeas(  1 :3,i)=sunmeas(  1 :3,i)/norm(sunmeas(  1 :3,i));  %  insure  noisy  sun  measurement  is  unit  vector 

unitmagmeas(l:3,i)=magmeas(l:3,i)/norm(magmeas(l:3,i));  %insure  noisy  mag  measurement  is  unit  vector 


vrot(l  :6,i)=[unitsunmeas(l  :3,i);unitmagmeas(l  :3,i)]; 


%form  vector  to  be  rotated  by  "best"  quaternion 


steps=10;  ^defines  max  allowable  iterations  for  convergence  within  Gauss_newton 

tol=.0 1 ;  ^defines  convergence  tolerance  for  Gauss_Newton 


[q_min(l:4,i),  A(l:4,l:6,i),  check(i),  err(i)]=Gauss_newton(vref(l:6,i),  vrot(l:6,i),  q_init,  steps,  tol); 


end 

end 


%******  Calculate  standard  deviation  of  error  in  measured  omegas  to  use  in  filter  R  matrices  -  not  available  in  real  world 
******* 


stdomega  1  meas=std(omega  1  measerror); 

stdomega2meas=std(omega2measenror);  %use  these  when  rate  measurements  are  available 

stdomega3meas=std(omega3measerror); 

%******  rotate  each  vector  using  actual  and  derived  attitude  quaternion  ****** 

%******  check  mean-square-error  between  vectors  ****** 

*******  Evaiuate  Mean-Square-Error  of  rotated  vectors  using  Gauss-Newton  derived  quaternion  ******* 

’Press  "return"  to  rotate  the  reference  vector  at  each  time  step  to  the' 
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'representation  in  the  rotated  frame  using  both  the  actual  rotation  quaternion' 
and  the  quaternion  derived  from  the  noisy  measurements... once  rotation  is  complete,' 

'calculate  the  mean-square-enror  between  the  results.' 
pause;cIose;c!c; 

GN_mse„error=[  ]; 
q„minvec=[  ]; 

Q_refvec-[  ]; 

for  j=l  .count; 

Q„refvec(  1 :6,j)=qframerot6(Qvec(  1 :4,j), vref(  1 :6  j));  %rotate  ref  vector  using  true  quaternion 

q_minvec(l  :6 ,j)=qframerot6(q_min(l  :4  j),vref(l  :6 ,j)>;  ^rotate  ref  vector  using  min  error  quaternion 

GN_mse„error(j)=mean_square_error(q_minvec(l:6,j),Q_refvec(l:6j));  %calculate  mse  between  rotated  vectors(exlude 

first  5) 

end 

The  average  mse  error  for  the  reference  vectors  rotated  using  the  GN-derived  quaternion  is:  ’ 
mean_GN_mse_error=mean(GN_mse_error) 

%******  Plot  mean-square-error  between  reference  vector  rotated  using  actual  quaternion  ****** 

%******  and  reference  vector  rotated  using  derived  quaternion.  ****** 

plot(T(  1  :count),GN_mse_error(  1  :count)),grid  on; 

titIe('MSE  between  reference  vectors  rotated  using  derived  and  actual  quaternions'); 
xlabel('time  (sec),);ylabel('MSE,);legend('MSE') 

'press  "enter”  to  continue...' 
pause;close;clc; 


%end  of  program 
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Appendix  B 

Representative  Plots  Available  From  Data  Simulation 

This  appendix  contains  plots  representative  of  those  generated  by  attitude„sim.m.  Many  additional 
numerical  outputs  are  displayed  on-screen,  and  the  display  of  plots  is  user-selectable.  The  plots  in  this 
appendix  were  generated  for  a  case  similar  to  the  baseline  simulation.  The  only  difference  is  that  a 
duration  of  10  seconds  is  used  instead  of  40  seconds  in  order  to  make  the  plots  more  clear  for  insertion  in 
this  text.  This  simulation  uses  the  baseline  sensor  measurement  standard  deviations  of  1.333  deg  in  each 
axis  for  the  Sun  sensor,  3.333  deg  in  each  axis  for  the  magnetometer,  and  0.333  rev/min  for  the  rate 
sensor.  The  rate  profile  in  this  case  asymptotically  approaches  the  final  values  of  0.5  rev/min  about  the 
body  x-axis,  0.5  rev/min  about  the  body  y-axis,  and  225  rev/min  about  the  body  z-axis  and  the  final  rates 
are  achieved  at  5  seconds. 

Figures  B.l  through  B.3  illustrate  the  generation  of  simulated  Sun  vector  components  and  the  noisy 
measurements  of  those  components.  This  representation  can  be  though  of  as  how  the  vector  “looks”  from 
the  rocket-mounted  body  frame  as  opposed  to  the  vector  in  inertial  coordinates. 


X  component  of  unit  sun  vector  vs.  time 


time  (sec) 


Figure  B.l:  Simulated  “x”  Component  of  Sun  Vector  -  Inertial,  Rotated,  Measured 


time  (sec) 


Figure  B.2:  Simulated  “y”  Component  of  Sun  Vector  -  Inertial,  Rotated,  Measured 


Z  component  of  unit  sun  vector  vs.  time 
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The  next  figure  is  a  three  dimensional  depiction  of  these  three  vector  components  of  the  rotated  vector 
over  time  and  the  inertial  vector. 


3-D  plot  of  both  Inertial  and  rotated  sun  vector  from  start  to  end  of  simulation 


+  rotated 
*  inertial 


Figure  B.4:  Three  Dimensional  Depiction  of  Inertial  and  Rotated  Sun  Vector 

The  four  figures  that  follow  are  a  representation  of  the  same  information  for  the  magnetic  field  vector.  For 
both  the  Sun  and  magnetic  field  vectors,  not  only  is  the  “true”  rotated  vector  generated,  but  also  the 
simulated  noisy  measurement  upon  which  the  filter  algorithms  operate.  The  noise  statistics  are  defined  by 
the  user  when  running  attitude_sim.m.  Figure  B.9  is  a  three  dimensional  view  of  both  the  Sun  and 
magnetic  field  vectors,  both  rotated  and  inertial.  It  also  displays  the  “vector”  part  of  the  rotation 
quaternion  at  each  time  step.  Figures  B.10  through  B.12  illustrate  the  simulated  body  rotational  rate 
profiles  as  well  as  the  simulated  noisy  measurement  of  the  rate  about  each  body  axis  at  each  time  step. 

In  addition  to  simulating  the  sensed  vectors,  attitude_sim.m  propagates  the  Euler  angles  forward  in  time 
and  then  converts  these  Euler  angles  to  an  attitude  quaternion  at  each  time  step.  This  provides  the  means 
to  generate  a  “truth”  with  which  to  compare  the  filter  estimates  to  assess  performance.  Figure  B.13 
depicts  the  Euler  angle  rates,  derived  from  the  body  rate  profiles.  This  is  followed  by  Figure  B.14,  the 
time  history  of  the  Euler  angles,  and  finally  by  the  time  history  of  the  attitude  quaternion  derived  from  the 
Euler  angles  in  Figure  B.15. 


time  (sec) 


Figure  B.5:  Simulated  “x”  Component  of  Magnetic  Field  Vector  -  Inertial,  Rotated,  Measured 


V  component  of  unit  mag  field  vector  vs.  time 


time  (sec) 


Figure  B.6:  Simulated  “y”  Component  of  Magnetic  Field  Vector  -  Inertial,  Rotated,  Measured 
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Figure  B.7:  Simulated  “z”  Component  of  Magnetic  Field  Vector  -  Inertial,  Rotated,  Measured 


3-D  plot  of  both  inertial  and  rotated  mag  field  vector  from  start  to  end  of  simulation 


Figure  B.8:  Three  Dimensional  Depiction  of  Inertial  and  Rotated  Magnetic  Field  Vector 
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3-D  plot  of  Inertial  and  rotated  sun  and  mag  field  vectors  and  rotation  quaternion 


Y  component  -1  -1  X  component 


Figure  B.9:  Three  Dimensional  View  of  Inertial  and  Rotated  Reference  Vectors  and  Quaternion 


Figure  B.10:  Simulated  ©i  Profile  and  Noisy  Measurements 


3 
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Appendix  C 

Representative  Plots  Available  from  Filter  Algorithms 

This  appendix  contains  plots  representative  of  those  generated  by  attitude_filter.m,  norates__ekf.m, 
rates_ekf.m,  norates_ukf.m  and  rates_ukf.m.  Many  additional  numerical  outputs  are  displayed  on-screen, 
and  the  display  of  plots  is  user-selectable.  The  plots  in  this  appendix  were  generated  for  a  case  similar  to 
the  baseline  simulation.  The  only  difference  is  that  a  duration  of  10  seconds  is  used  instead  of  40  seconds 
in  order  to  make  the  plots  more  clear  for  insertion  in  this  text.  This  simulation  uses  the  baseline  sensor 
measurement  standard  deviations  of  1.333  deg  in  each  axis  for  the  Sun  sensor,  3.333  deg  in  each  axis  for 
the  magnetometer,  and  0.333  rev/min  for  the  rate  sensor.  The  rate  profile  used  asymptotically  approaches 
the  final  values  of  0.5  rev/min  about  the  body  x-axis,  0.5  rev/min  about  the  body  y-axis,  and  225  rev/min 
about  the  body  z-axis  and  the  final  rates  are  achieved  at  5  seconds. 

The  first  four  figures  depict  the  Gauss-Newton  derived  quaternion  components  versus  the  “true” 
quaternions  generated  from  the  attitude_sim.m  propagated  Euler  angles.  The  solid  lines  are  the  true  values 
and  the  “+”  symbols  are  the  components  derived  from  the  error-minimization  step. 


derived  ql  vs.  Euler-based  ql 


Figure  C.l:  Gauss-Newton  Derived  qi  Versus  True  qi 
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Figure  C.4:  Gauss-Newton  Derived  q4  Versus  True  q4 


Figure  C.5  is  a  three-dimensional  depiction  of  the  reference  vector  rotated  using  the  actual  quaternion 
from  attitude_sim.m  and  using  the  quaternion  from  the  Gauss-Newton  error  minimization.  The  next  figure 
gives  an  illustration  of  the  mean  square  error  at  each  time  step  between  these  two  rotated  vectors.  This  is  a 
visual  cue  to  how  well  the  error  minimization  is  performing.  These  two  figures  are  then  followed  by  plots 
of  the  error  in  the  UKF  estimates  of  the  rotational  rates.  The  theoretical  bounds  on  the  error  come  from 
the  diagonal  element  of  the  covariance  matrix  that  corresponds  to  that  state.  One  good  indication  of 
proper  filter  performance  for  a  Kalman  style  filter  is  that  at  least  68%  of  the  error  estimates  fall  within  the 
theoretical  bounds  [17].  Following  are  figures  showing  the  error  in  the  UKF  estimate  of  each  quaternion 
component,  again  with  the  corresponding  theoretical  bounds.  Next,  the  filter  estimate  of  each  rate,  its 
“measurements”  and  the  true  rotational  rate  are  plotted  for  each  axis  in  Figures  C.14  through  C.16.  These 
give  a  visual  indication  of  the  quality  of  the  UKF  rate  estimates.  For  a  similar  visual  assessment  of  the 
quality  of  the  quaternion  estimate,  the  next  figure  shows  the  reference  vectors  rotated  using  the  UKF 
derived  quaternion  versus  that  rotated  using  the  true  quaternion  and  Figure  C.18  displays  the  mean  square 
error  between  the  vectors  rotated  using  these  two  quaternions.  The  final  figure  depicts  the  body  frame  unit 
vectors  rotated  in  the  inertial  frame  using  both  the  true  and  the  filter  generated  quaternions.  This  gives  a 
visual  interpretation  of  the  “pointing  accuracy”  error  of  the  solution.  These  plots  were  generated  using  the 
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Figure  C.5:  Reference  Vectors  Rotated  Using  Gauss-Newton  Derived  and  True  Quaternions 


time  (sec) 


Figure  C.6:  MSE  Between  Vectors  Rotated  Using  Gauss-Newton  and  True  Quaternions 


Figure  C.7:  Error  in  the  UKF  Estimate  of  the  Rotational  Rate  About  the  “x”  Body  Axis 


Figure  C.8:  Error  in  the  UKF  Estimate  of  the  Rotational  Rate  About  the  “y”  Body  Axis 


time  (sec) 


Figure  C.9:  Error  in  the  UKF  Estimate  of  the  Rotational  Rate  About  the  “z”  Body  Axis 


Figure  C.  10:  Error  in  the  UKF  Estimate  of  the  qi  Component  of  the  Quaternion 


err<*  in  q3 


Figure  C.ll:  Error  in  the  UKF  Estimate  of  the  q2  Component  of  the  Quaternion 


Figure  C.12:  Error  in  the  UKF  Estimate  of  the  q3  Component  of  the  Quaternion 


Figure  C.  13:  Error  in  the  UKF  Estimate  of  the  q4  Component  of  the  Quaternion 


Figure  C.14:  UKF  Estimate  of  ©i  Versus  Measured  ©!  Versus  True  ©i 


3-D  plot  of  reference  vectors  rotated  using  actual  quaternions  and  ukf  derived  quaternions 


Y  component  -1  -1  X  component 


Figure  C.17:  Reference  Vectors  Rotated  Using  Gauss-Newton  Derived  and  True  Quaternions 


Figure  C.18:  MSE  Between  Vectors  Rotated  Using  UKF  derived  and  True  Quaternions 
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UKF  algorithm  “with  rates  available.”  Similar  plots  are  generated  for  the  EKF  algorithms  with  and 
without  rate  measurements,  and  for  the  UKF  algorithm  without  rates. 

At  the  end  of  each  filtering  run,  the  user  has  the  option  to  calculate  the  angles  between  the  body  axes 
rotated  using  the  “true”  quaternion  and  the  body  axes  rotated  using  the  filter  estimated  quaternion.  In  each 
case,  the  body  frame  is  represented  in  inertial  coordinates.  A  second  option  allows  a  choice  of  whether  to 
plot  the  time  step  history  of  these  results  along  with  displaying  the  mean,  standard  deviation,  and 
maximum  of  each,  or  to  simply  display  the  summary  values.  Figure  C.19  is  an  example  of  the  plot 
produced  if  that  option  is  selected. 


3D  plot  of  “true"  and  “estimated"  body  axes  in  inertial  frame 


Figure  C.19:  3D  Plot  of ‘True”  and  ’’Estimated  Body  Axes  in  the  Inertial  Frame 
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Appendix  D 

Sample  Results  Used  to  Determine  Process  Noise  and  Time  Constants 

The  following  table  contains  the  data  generated  while  “tuning”  the  four  filters  as  described  in  Chapter  10. 
This  particular  simulation  was  40  seconds  in  duration  and  used  the  baseline  sensor  measurement  standard 
deviations.  The  rate  profile  about  each  axis  asymptotically  approaches  the  baseline  values  of  0.5  rev/min 
about  the  x-axis,  0.5  rev/min  about  the  y-axis,  and  225  rev/min  about  the  z-axis.  The  final  rates  are 
achieved  at  20  seconds.  All  numerical  integration  was  accomplished  using  a  0.01  sec  step  size  unless 
otherwise  noted  in  the  table,  and  the  UKF  parameters  a  and  P  were  set  to  0.01  and  2  unless  otherwise 
noted.  GN-MSE  is  the  average  mean  square  error  between  the  reference  vector  rotated  using  the  true 
quaternion  and  that  rotated  using  the  Gauss-Newton  generated  quaternion.  GN-FOM  is  the  figure  of  merit 
for  the  Gauss-Newton  routine  as  described  in  Chapter  10,  PHIS  is  the  process  noise,  Taul/2  is  the  time 
constant  corresponding  to  ©!  and  ©2  while  Tau3  is  the  time  constant  corresponding  to  03.  EKF-MSE  and 
UKF-MSE  are  the  average  mean  square  error  between  the  reference  vector  rotated  using  the  filter  and  that 
rotated  using  the  true  quaternion  for  the  EKF  and  the  UKF  respectively.  EKF-FOM  and  UKF-FOM  are 
the  overall  figure  of  merit,  as  defined  in  Chapter  10,  for  the  EKF  and  the  UKF  respectively. 

Table  D.l:  Sample  Data  From  Tuning  the  Filters  for  One  Simulation  Case 


Rate  Profile:  40  sec  simulation .  sun  sensor  sigma  =  1.333  deg,  maa  sensor  siama  =  3.333  decj, 

rate  sensor  sigma  0.333  rom.  asymptotic  approach  to  final  rom  at  20  sec , 
step  size  -  .01  sec  tunless  otherwise  noted  bv  PHIS/timesteo) 

Without  rate  measurements  available 


GN-MSE 


0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

GN-FOM 

PHIS 

Tau  1/2 

Tau  3 

EKF-MSE 

UKF-MSE 

EKF-FOM 

UKF-FOM 

37.3090 

1.86 

31.00 

1.00E+07 

9.99E-04 

0.1061 

37.3090 

3.80 

28.00 

1.00E+07 

0.0010 

0.0692 

37.3090 

3.82 

28.00 

1.00E+07 

0.0010 

0.0693 

37.3090 

3.78 

1.00E+07 

0.0010 

0.0692 

37.3090 

3.75 

1.00E+07 

0.0010 

0.0691 

37.3090 

3.60 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.55 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.50 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.40 

28.00 

1.00E+07 

0.0010 

0.0689 

37.3090 

3.41 

28.00 

1.00E+07 

0.0010 

0.0689 

37.3090 

3.45 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.44 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.43 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.42 

28.00 

1.00E+07 

0.0010 

0.0688 
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Table  D.l:  Sample  Data  From  Tuning  the  Filters  for  One  Simulation  Case  (cont.) 


37.3090 

3.65 

28.00 

1.00E+07 

0.0010 

0.0689 

37.3090 

3.61 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.62 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.63 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.64 

28.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.53 

31.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.53 

34.00 

1.00E+07 

0.0010 

0.0689 

37.3090 

3.53 

33.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.53 

25.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.53 

20.00 

1.00E+07 

0.0010 

0.0692 

37.3090 

3.53 

24.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.53 

23.00 

1.00E+07 

0.0010 

0.0689 

37.3090 

3.53 

29.00 

1.00E+10 

0.0010 

0.0688 

37.3090 

3.53 

29.00 

1.00E+03 

0.0010 

0.0694 

37.3090 

3.53 

29.00 

5.00E+03 

0.0010 

0.0689 

37.3090 

3.53 

29.00 

1.00E+04 

0.0010 

0.0688 

37.3090 

3.53/.001 

29.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.53/.0001 

29.00 

1.00E+07 

0.0010 

0.0688 

37.3090 

3.53/.  1 

29.00 

1.00E+07 

0.0010 

3.963 

37.3090 

2.15 

0.07 

1.00E+07 

0.0010 

0.3151 

37.3090 

2.00 

0.07 

1.00E+07 

0.0010 

0.2414 

37.3090 

1.86 

0.07 

1.00E+07 

0.0010 

0.1849 

37.3090 

1.00 

0.07 

1.00E+07 

0.0010 

0.0461 

37.3090 

0.75 

0.07 

1.00E+07 

0.0010 

0.0887 

37.3090 

0.90 

0.07 

1.00E+07 

0.0010 

0.054 

37.3090 

1.10 

0.07 

1.00E+07 

0.0010 

0.0452 

37.3090 

1.11 

0.07 

1.00E+07 

0.0010 

0.0454 

37.3090 

1.09 

0.07 

1.00E+07 

0.0010 

0.045 

37.3090 

1.08 

0.07 

1.00E+07 

0.0010 

0.0449 

37.3090 

1.07 

0.07 

1.00E+07 

0.0010 

0.0449 

37.3090 

1.06 

0.07 

1.00E+07 

0.0010 

0.0449 

37.3090 

1.05 

0.07 

1.00E+07 

0.0010 

0.0449 

37.3090 

1.04 

0.07 

1.00E+07 

0.0010 

0.045 

37.3090 

1.1/.001 

0.07 

1.00E+07 

0.0010 

0.0452 

37.3090 

1.1/.0001 

0.07 

1.00E+07 

0.0010 

0.0452 

37.3090 

1.1/.1 

0.07 

1.00E+07 

0.0010 

0.0684 

37.3090 

1.07 

0.06 

1.00E+07 

0.0010  ! 

0.0449 

37.3090 

1.07 

0.05 

1.00E+07 

0.0010  1 

0.0449 

37.3090 

1.07 

0.08 

1.00E+07 

0.0010 

0.0449 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 
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Table  D.l:  Sample  Data  From  Tuning  the  Filters  for  One  Simulation  Case  (cont.) 


0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 

0.0010 


RISI 

1.07 

0.04 

1.00E+07 

0.0010 

0.00451 

37.3090 

1.07 

0.09 

1.00E+07 

0.0010 

0.0449 

37.3090 

1.07 

0.1 

1.00E+07 

0.0010 

0.0449 

37.3090 

1.07 

0.11 

1.00E+07 

0.0010 

0.045 

37.3090 

1.07 

0.07 

1.00E+05 

0.0010 

0.0449 

37.3090 

1.07 

0.07 

1.00E+03 

0.0010 

0.0452 

37.3090 

1.07 

0.07 

1.00E+10 

0.0010 

0.0449 

37.3090 

1.07 

0.07 

1.00E+04 

0.0010 

0.0449 

1.07 

0.07 

5.00E+03 

0.0010 

0.0449 

With  rate  measurements  available 


GN-MSE 

GN- 

FOM 

PHIS 

Tau 

1/2 

Tau  3 

EKF- 

MSE 

UKF- 

MSE 

EKF- 

FOM 

UKF- 

FOM 

0.0010 

0.0016 

36.00 

1.00E+07 

0.001 

0.0012 

0.0010 

0.0016 

36.00 

36.00 

1.00E+07 

0.001 

0.0011 

0.0010 

0.0016 

38.00 

1.00E+07 

0.001 

0.0011 

0.0010 

0.0016 

40.00 

1.00E+07 

0.001 

0.0010 

0.0016 

42.00 

36.00 

1.00E+07 

0.001 

0.0011 

0.0010 

0.0016 

44.00 

36.00 

1.00E+07 

0.001 

0.001 1 

0.0010 

0.0016 

46.00 

1.00E+07 

0.001 

0.0011 

0.0010 

0.0016 

48.00 

36.00 

1.00E+07 

0.001 

0.0011 

0.0010 

0.0016 

50.00 

36.00 

1.00E+07 

0.001 

0.0011 

0.0010 

0.0016 

52.00 

36.00 

1.00E+07 

0.001 

0.0011 

0.0010 

BlilB 

60.00 

36.00 

1.00E+07 

0.001 

0.0010 

0.0016 

65.00 

36.00 

1.00E+07 

0.001 

0.0011 

0.0010 

0.0016 

70.00 

36.00 

1.00E+07 

9.80E-04 

0.0011 

0.0010 

0.0016 

80.00 

36.00 

1.00E+07 

9.78E-04 

0.0011 

0.0010 

0.0016 

100.00 

36.00 

1.00E+07 

0.0010 

36.00 

1.00E+07 

9.69E-04 

0.0011 

0.0010 

0.0016 

36.00 

1.00E+07 

9.68E-04 

0.0012 

0.0016 

36.00 

1.00E+07 

9.68E-04 

0.0011 

0.0010 

0.0016 

180.00 

36.00 

1.00E+07 

9.68E-04 

0.0011 

0.0010 

0.0016 

185.00 

36.00 

1.00E+07 

9.68E-04 

0.0011 

0.0010 

0.0016 

190.00 

36.00 

1.00E+07 

9.68E-04 

0.0012 

0.0010 

0.0016 

112.00 

40.00 

1.00E+07 

9.72E-04 

0.0011 

0.0010 

0.0016 

112.00 

45.00 

1.00E+07 

9.72E-04 

0.0011 

0.0010 

0.0016 

112.00 

1.00E+07 

9.72E-04 

0.0011 

0.0010 

0.0016 

112.00 

55.00 

1.00E+07 

9.72E-04 

0.001 1 

0.0010 

0.0016 

112.00 

30.00 

1.00E+07 

I5335I 

0.0011 

0.0010 

0.0016 

112.00 

60.00 

1.00E+07 

9.72E-04 

0.0011 

0.0010 

■ 

kS&sIIIII 

112.00 

25.00 

1.00E+07 

9.72E-04 

0.0011 

0.0010 

0.0016 

112.00 

65.00 

1.00E+07 

9.72E-04 

0.0011 

0.0010 

0.0016 

112.00 

20.00 

1.00E+07 

9.72E-04 

0.0011 
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Table  D.l:  Sample  Data  From  Tuning  the  Filters  for  One  Simulation  Case  (cont.) 


0.0016 

.023/.0001 

0.20 

KPI 

0.0016 

.023/.  1 

0.20 

1.00E+07 

0.0831 

0.0831 

Same  GN-EKF  and  GN-FOM  as  above .  but  varying  a  and  B 


3=2 

0.023 

1.00E+07 

4.48E-04 

6.37E-04 

3=2 

0.023 

0.20 

1.00E+07 

4.48E-04 

6.37E-04 

3=2 

0.023 

1.00E+07 

4.48E-04 

6.37E-04 

3=2 

0.023 

1.00E+07 

4.48E-04 

6.37E-04 

B9 

0.023 

0.20 

1.00E+07 

4.48E-04 

6.37E-04 

3=2 

0.023 

0.20 

1.00E+07 

4.48E-04 

6.37E-04 

8=5 

0.023 

0.20 

1.00E+07 

4.48E-04 

6.37E-04 

lH3H 

0.023 

0.20 

1.00E+07 

4.48E-04 

6.37E-04 

Same  data  set  as  above,  drop  out  all  measurements  for  1  sec  beginning  at  10  sec 
Without  rate  measurements  available 


0.0114 

0.0114 


39.5700 

3.530 

29.00 

1.00E+07 

39.5700 

1.070 

0.07 

1.00E+07 

0.0116 

0.0570 

Same  data  set  as  above,  drop  out  ail  measurements  for  1  sec  beginning  at  10  sec 
With  rate  measurements  available 


4.024 

112.000 

36.00 

1.00E+07 

0.0173 

1,1627 

4.024 

0.023 

0.20 

1.00E+07 

0.0103 

0.4969 

Same  data  set  as  above,  bias  of  -0.1  rom  on  al!  rate  measurements 
Without  rate  measurements  available 


0.0010 

0.0010 


37.310 

3.530 

29.00 

1.00E+07 

0.0010 

0.0688 

37.310 

1.070 

0.07 

1.00E+07 

0.0010 

0.0449 

Same  data  set  as  above,  bias  of -0.1  rom  on  all  rate  measurements 
With  rate  measurements  available 


0.0112 

112.000 

36.00 

1.00E+07 

9.70E-04 

0.0108 

0.0112 

0.023 

0.20 

1.00E+07 

0.0004 

0.0105 

Same  data  set  as  above,  bias  of  +0.1  deg  on  each  axis  of  sun  sensor,  and  then  vector  normalized 
Without  rate  measurements  available 


0.0035 

0.0035 


40.440 

3.530 

29.00 

1.00E+07 

0.0035 

0.2081 

40.440 

1.070 

0.07 

1.00E+07 

0.0035 

0.0952 
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Table  D.l:  Sample  Data  From  Tuning  the  Filters  for  One  Simulation  Case  (cont.) 


Same  data  set  as  above .  bias  of +0.1  deg  on  each  axis  of  sun  sensor,  and  then  vector  normalized 
With  rate  measurements  available 


0,0037 

0.0037 


0.0037 

112.000 

36.00 

1.00E+07 

0,0034 

0.0035 

0.0037 

0.023 

0.20 

1.00E+07 

0.0029 

0.0029 

